diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ + diff --git a/LICENSE b/LICENSE index 92f784cc..f37a869e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,29 +1,62 @@ -BSD 3-Clause License - -Copyright (c) 2018, Kinova Robotics +Copyright (c) 2018, Kinova inc. All rights reserved. +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: +* Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. +* Neither the name of the copyright holder nor the names of its contributors +may be used to endorse or promote products derived from this software +without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +____________________________________________________________________ + + +Protocol Buffer license + +Copyright 2008 Google Inc. All rights reserved. Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: +modification, are permitted provided that the following conditions are +met: -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. + * Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above +copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the +distribution. + * Neither the name of Google Inc. nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Code generated by the Protocol Buffer compiler is owned by the owner +of the input file used when generating it. This code is not +standalone and requires a support library to be linked with it. This +support library is itself covered by the above license. \ No newline at end of file diff --git a/kortex_actuator_driver/CMakeLists.txt b/kortex_actuator_driver/CMakeLists.txt new file mode 100644 index 00000000..7861e100 --- /dev/null +++ b/kortex_actuator_driver/CMakeLists.txt @@ -0,0 +1,49 @@ + +cmake_minimum_required(VERSION 2.8.3) +project(kortex_actuator_driver) + +add_compile_options(-std=c++11) +add_definitions(-D_OS_UNIX) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) + +file(GLOB_RECURSE cpp_list RELATIVE ${PROJECT_SOURCE_DIR} "src/*.cpp") + +## Declare ROS messages and services +add_message_files(DIRECTORY msg) +add_message_files(DIRECTORY msg/non_generated) +add_service_files(DIRECTORY srv) +add_service_files(DIRECTORY srv/non_generated) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${PROJECT_SOURCE_DIR}/src) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client_stubs) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/messages) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/common) +include_directories(include ${PROJECT_SOURCE_DIR}/src/util) + +link_directories(${PROJECT_SOURCE_DIR}/../kortex_api/lib/release) + +add_executable(${PROJECT_NAME} ${cpp_list}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} CppKinovaApi gcov) + +find_package(Protobuf 3.5.1 REQUIRED) + +if(Protobuf_FOUND) + target_include_directories(${PROJECT_NAME} PUBLIC ${PROTOBUF_INCLUDE_DIRS}) + target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES}) +else() + message(FATAL_ERROR "Protobuf libraries not found") +endif() + +add_dependencies(${PROJECT_NAME} kortex_actuator_driver_gencpp) + diff --git a/kortex_actuator_driver/RosGeneration.py b/kortex_actuator_driver/RosGeneration.py new file mode 100644 index 00000000..f47a0d01 --- /dev/null +++ b/kortex_actuator_driver/RosGeneration.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python + +import sys + +from google.protobuf.compiler import plugin_pb2 as plugin +from google.protobuf import json_format as json_f + +import jinja2 + +import itertools +import json +import types +import os +import sys + +from google.protobuf.descriptor_pb2 import DescriptorProto, EnumDescriptorProto, ServiceDescriptorProto, FieldDescriptorProto, OneofDescriptorProto + +#Class that holds a protobuf message and some other details needed by the generator(jinja2 template). +class DetailedMessage: + def __init__(self, message=None): + self.message = message + self.HasOneOf = "false" + self.oneOfList = [] + +#Class that holds a protobuf service and some other details needed by the generator(jinja2 template). +class DetailedPackage: + def __init__(self, service=None): + self.name = "NoName" + self.service = service + +#JINJA2 function to render a file from a template. +def render(tpl_path, context): + path, filename = os.path.split(tpl_path) + return jinja2.Environment(loader=jinja2.FileSystemLoader(path or './')).get_template(filename).render(**context) + +#Main plugin function +def generate_code(request, response): + + #The context is the object sent to the JINJA2 template + context = types.SimpleNamespace() + context.serviceVersion = 1 + + context.detailedPackages = [] + + MainFilePath = os.path.join(".", "src/main.cpp") + function_list = [] + fileIndex = 0 + + for proto_file in request.proto_file: + context.detailedPackages.append(DetailedPackage()) + context.detailedPackages[fileIndex].name = proto_file.package.split(".")[-1] + context.detailedPackages[fileIndex].filename = proto_file.name.split(".")[0] + context.detailedPackages[fileIndex].namespace = proto_file.package.replace(".", "::") + context.detailedPackages[fileIndex].HasRPC = 0 + context.detailedPackages[fileIndex].HasMessage = 0 + + HeaderFilePath = os.path.join(".", "src/node.h") + CppFilePath = os.path.join(".", "src/node.cpp") + + #We lower the case to respect ROS coding standard style + CppProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.h".format(proto_file.name.split(".")[0].lower())) + CppRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.h".format(proto_file.name.split(".")[0].lower())) + + list_detailedMessage = [] + list_detailedMethod = [] + + # For every item in the current proto file + for item, package in traverse(proto_file): + context.HasOneOf = 0 + + + if isinstance(item, EnumDescriptorProto): + context.item = item + + ros_enumPath = os.path.join(".", "msg/{}.msg".format(item.name)) + + with open(ros_enumPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_enum.jinja2", context.__dict__)) + #If this it a message + if isinstance(item, DescriptorProto): + tempMessage = DetailedMessage(item) + context.detailedPackages[fileIndex].HasMessage = 1 + + for member in item.field: + #If a member is part of a oneof, it will have this additional field + if member.HasField("oneof_index"): + context.HasOneOf = 1 + tempMessage.HasOneOf = "true" + else: + context.HasOneOf = 0 + tempMessage.HasOneOf = "false" + + context.item = item + + #If the proto file contains a ONEOF we need to generate a separate file to handle it. + if context.HasOneOf == 1: + + #This line gets the list of ONEOF that is in the current message. + oneOfList = item.ListFields()[-1][1] + + tempMessage.oneOfList = item.ListFields()[-1][1] + ros_oneofPath = os.path.join(".", "msg/{}_{}.msg".format(item.name, oneOfList[0].name)) + + with open(ros_oneofPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_oneof.jinja2", context.__dict__)) + + + list_detailedMessage.append(tempMessage) + ros_messagePath = os.path.join(".", "msg/{}.msg".format(item.name)) + + #We call jinja2 to generate a ROS message. + with open(ros_messagePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_message.jinja2", context.__dict__)) + + #If this is a service (A group of method) + if isinstance(item, ServiceDescriptorProto): + for method in item.method: + context.item = method + if "Topic" not in method.name: + function_list.append(method.name) + ros_servicePath = os.path.join(".", "srv/{}.srv".format(method.name)) + with open(ros_servicePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_service.jinja2", context.__dict__)) + + context.detailedPackages[fileIndex].service = item + context.detailedPackages[fileIndex].HasRPC = 1 + + context.currentPackageName = context.detailedPackages[fileIndex].name + context.currentNamespace = proto_file.package.replace(".", "::") + context.currentFilename = context.detailedPackages[fileIndex].filename + context.item = list_detailedMessage + + if context.detailedPackages[fileIndex].HasMessage == 1: + #Wecall jinja2 to generate a prot/ROS converter for every protobuf message. + with open(CppProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.cpp.jinja2", context.__dict__)) + with open(HeaderProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.h.jinja2", context.__dict__)) + with open(CppRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.cpp.jinja2", context.__dict__)) + with open(HeaderRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.h.jinja2", context.__dict__)) + + fileIndex = fileIndex + 1 + + context.list_function = function_list + + #We jinja2 to generate the ROS node. + with open(HeaderFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.h.jinja2", context.__dict__)) + with open(CppFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.cpp.jinja2", context.__dict__)) + with open(MainFilePath, 'wt') as mainFile: + mainFile.write(render("./templates/main.jinja2", context.__dict__)) + +def traverse(proto_file): + #recursive function that browse a protobof item + def _traverse(package, items): + for item in items: + yield item, package + + if isinstance(item, DescriptorProto): + for enum in item.enum_type: + yield enum, package + + for nested in item.nested_type: + nested_package = package + item.name + + for nested_item in _traverse(nested, nested_package): + yield nested_item, nested_package + if isinstance(item, ServiceDescriptorProto): + for rpc in item.method: + yield rpc, package + + #return a list of everything found in the proto file + return itertools.chain( + _traverse(proto_file.package, proto_file.enum_type), + _traverse(proto_file.package, proto_file.message_type), + _traverse(proto_file.package, proto_file.service), + ) + +if __name__ == '__main__': + # Read request message from stdin + data = sys.stdin.buffer.read() + + # Parse request + request = plugin.CodeGeneratorRequest() + request.ParseFromString(data) + + # Create response + response = plugin.CodeGeneratorResponse() + + # Generate code + generate_code(request, response) + + # Serialise response message + output = response.SerializeToString() + + # Write to stdout + sys.stdout.buffer.write(output) \ No newline at end of file diff --git a/kortex_actuator_driver/build/.gitignore b/kortex_actuator_driver/build/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_actuator_driver/kortex_actuator_driver.sh b/kortex_actuator_driver/kortex_actuator_driver.sh new file mode 100755 index 00000000..011eee96 --- /dev/null +++ b/kortex_actuator_driver/kortex_actuator_driver.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +python3 -u RosGeneration.py + diff --git a/kortex_actuator_driver/msg/ArmState.msg b/kortex_actuator_driver/msg/ArmState.msg new file mode 100644 index 00000000..d85bd69d --- /dev/null +++ b/kortex_actuator_driver/msg/ArmState.msg @@ -0,0 +1,22 @@ + +uint32 UNSPECIFIED_ARM_STATE = 0 + +uint32 BASE_INITIALIZATION = 1 + +uint32 IDLE = 2 + +uint32 ARM_INITIALIZATION = 3 + +uint32 ARM_IN_FAULT = 4 + +uint32 ARM_MAINTENANCE = 5 + +uint32 ARM_SERVOING_LOW_LEVEL = 6 + +uint32 ARM_SERVOING_READY = 7 + +uint32 ARM_SERVOING_PLAYING_SEQUENCE = 8 + +uint32 ARM_SERVOING_MANUALLY_CONTROLLED = 9 + +uint32 RESERVED = 255 diff --git a/kortex_actuator_driver/msg/AxisOffsets.msg b/kortex_actuator_driver/msg/AxisOffsets.msg new file mode 100644 index 00000000..64f67904 --- /dev/null +++ b/kortex_actuator_driver/msg/AxisOffsets.msg @@ -0,0 +1,3 @@ + +float32 absolute_offset +float32 relative_offset \ No newline at end of file diff --git a/kortex_actuator_driver/msg/AxisPosition.msg b/kortex_actuator_driver/msg/AxisPosition.msg new file mode 100644 index 00000000..380da9af --- /dev/null +++ b/kortex_actuator_driver/msg/AxisPosition.msg @@ -0,0 +1,2 @@ + +float32 position \ No newline at end of file diff --git a/kortex_actuator_driver/msg/Command.msg b/kortex_actuator_driver/msg/Command.msg new file mode 100644 index 00000000..25525fa5 --- /dev/null +++ b/kortex_actuator_driver/msg/Command.msg @@ -0,0 +1,8 @@ + + +MessageId command_id +uint32 flags +float32 position +float32 velocity +float32 torque_joint +float32 current_motor \ No newline at end of file diff --git a/kortex_actuator_driver/msg/CommandMode.msg b/kortex_actuator_driver/msg/CommandMode.msg new file mode 100644 index 00000000..fad4f4d3 --- /dev/null +++ b/kortex_actuator_driver/msg/CommandMode.msg @@ -0,0 +1,12 @@ + +uint32 CYCLIC = 0 + +uint32 ASYNC_CYCLIC_FLAGS = 1 + +uint32 ASYNC = 2 + +uint32 CYCLIC_JITTERCOMPENSATED_POSITION = 3 + +uint32 CYCLIC_JITTERCOMPENSATED_VELOCITY = 4 + +uint32 CYCLIC_JITTERCOMPENSATED_ACCELERATION = 5 diff --git a/kortex_actuator_driver/msg/CommandModeInformation.msg b/kortex_actuator_driver/msg/CommandModeInformation.msg new file mode 100644 index 00000000..e6342c48 --- /dev/null +++ b/kortex_actuator_driver/msg/CommandModeInformation.msg @@ -0,0 +1,3 @@ + + +uint32 command_mode \ No newline at end of file diff --git a/kortex_actuator_driver/msg/Connection.msg b/kortex_actuator_driver/msg/Connection.msg new file mode 100644 index 00000000..ca08b6c8 --- /dev/null +++ b/kortex_actuator_driver/msg/Connection.msg @@ -0,0 +1,5 @@ + + +UserProfileHandle user_handle +string connection_information +uint32 connection_identifier \ No newline at end of file diff --git a/kortex_actuator_driver/msg/ControlLoop.msg b/kortex_actuator_driver/msg/ControlLoop.msg new file mode 100644 index 00000000..17b6d695 --- /dev/null +++ b/kortex_actuator_driver/msg/ControlLoop.msg @@ -0,0 +1,2 @@ + +uint32 control_loop \ No newline at end of file diff --git a/kortex_actuator_driver/msg/ControlLoopParameters.msg b/kortex_actuator_driver/msg/ControlLoopParameters.msg new file mode 100644 index 00000000..72b8bbe5 --- /dev/null +++ b/kortex_actuator_driver/msg/ControlLoopParameters.msg @@ -0,0 +1,7 @@ + + +uint32 loop_selection +float32 error_saturation +float32 output_saturation +float32[] kAz +float32[] kBz \ No newline at end of file diff --git a/kortex_actuator_driver/msg/ControlLoopSelection.msg b/kortex_actuator_driver/msg/ControlLoopSelection.msg new file mode 100644 index 00000000..6b620930 --- /dev/null +++ b/kortex_actuator_driver/msg/ControlLoopSelection.msg @@ -0,0 +1,14 @@ + +uint32 RESERVED = 0 + +uint32 JOINT_POSITION = 1 + +uint32 MOTOR_POSITION = 2 + +uint32 JOINT_VELOCITY = 4 + +uint32 MOTOR_VELOCITY = 8 + +uint32 JOINT_TORQUE = 16 + +uint32 MOTOR_CURRENT = 32 diff --git a/kortex_actuator_driver/msg/ControlMode.msg b/kortex_actuator_driver/msg/ControlMode.msg new file mode 100644 index 00000000..7f80fdc4 --- /dev/null +++ b/kortex_actuator_driver/msg/ControlMode.msg @@ -0,0 +1,12 @@ + +uint32 NONE = 0 + +uint32 POSITION = 1 + +uint32 VELOCITY = 2 + +uint32 TORQUE = 3 + +uint32 CURRENT = 4 + +uint32 CUSTOM = 5 diff --git a/kortex_actuator_driver/msg/ControlModeInformation.msg b/kortex_actuator_driver/msg/ControlModeInformation.msg new file mode 100644 index 00000000..3bcfa3fa --- /dev/null +++ b/kortex_actuator_driver/msg/ControlModeInformation.msg @@ -0,0 +1,3 @@ + + +uint32 control_mode \ No newline at end of file diff --git a/kortex_actuator_driver/msg/CustomData.msg b/kortex_actuator_driver/msg/CustomData.msg new file mode 100644 index 00000000..b81c1b26 --- /dev/null +++ b/kortex_actuator_driver/msg/CustomData.msg @@ -0,0 +1,19 @@ + + +MessageId custom_data_id +uint32 custom_data_0 +uint32 custom_data_1 +uint32 custom_data_2 +uint32 custom_data_3 +uint32 custom_data_4 +uint32 custom_data_5 +uint32 custom_data_6 +uint32 custom_data_7 +uint32 custom_data_8 +uint32 custom_data_9 +uint32 custom_data_10 +uint32 custom_data_11 +uint32 custom_data_12 +uint32 custom_data_13 +uint32 custom_data_14 +uint32 custom_data_15 \ No newline at end of file diff --git a/kortex_actuator_driver/msg/CustomDataSelection.msg b/kortex_actuator_driver/msg/CustomDataSelection.msg new file mode 100644 index 00000000..1197aaba --- /dev/null +++ b/kortex_actuator_driver/msg/CustomDataSelection.msg @@ -0,0 +1,2 @@ + +uint32[] channel \ No newline at end of file diff --git a/kortex_actuator_driver/msg/DeviceHandle.msg b/kortex_actuator_driver/msg/DeviceHandle.msg new file mode 100644 index 00000000..49f84366 --- /dev/null +++ b/kortex_actuator_driver/msg/DeviceHandle.msg @@ -0,0 +1,5 @@ + + +uint32 device_type +uint32 device_identifier +uint32 order \ No newline at end of file diff --git a/kortex_actuator_driver/msg/DeviceTypes.msg b/kortex_actuator_driver/msg/DeviceTypes.msg new file mode 100644 index 00000000..5a55df07 --- /dev/null +++ b/kortex_actuator_driver/msg/DeviceTypes.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_DEVICE_TYPE = 0 + +uint32 BASE = 1 + +uint32 VISION = 2 + +uint32 BIG_ACTUATOR = 3 + +uint32 SMALL_ACTUATOR = 4 + +uint32 INTERCONNECT = 5 + +uint32 GRIPPER = 6 diff --git a/kortex_actuator_driver/msg/Empty.msg b/kortex_actuator_driver/msg/Empty.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_actuator_driver/msg/EncoderDerivativeParameters.msg b/kortex_actuator_driver/msg/EncoderDerivativeParameters.msg new file mode 100644 index 00000000..c83b9929 --- /dev/null +++ b/kortex_actuator_driver/msg/EncoderDerivativeParameters.msg @@ -0,0 +1,3 @@ + +uint32 max_window_width +uint32 min_encoder_tick_count \ No newline at end of file diff --git a/kortex_actuator_driver/msg/Feedback.msg b/kortex_actuator_driver/msg/Feedback.msg new file mode 100644 index 00000000..9698d184 --- /dev/null +++ b/kortex_actuator_driver/msg/Feedback.msg @@ -0,0 +1,16 @@ + + +MessageId feedback_id +uint32 status_flags +uint32 jitter_comm +float32 position +float32 velocity +float32 torque +float32 current_motor +float32 voltage +float32 temperature_motor +float32 temperature_core +uint32 fault_bank_a +uint32 fault_bank_b +uint32 warning_bank_a +uint32 warning_bank_b \ No newline at end of file diff --git a/kortex_actuator_driver/msg/FrequencyResponse.msg b/kortex_actuator_driver/msg/FrequencyResponse.msg new file mode 100644 index 00000000..ea3ce5bd --- /dev/null +++ b/kortex_actuator_driver/msg/FrequencyResponse.msg @@ -0,0 +1,7 @@ + + +uint32 loop_selection +float32 min_frequency +float32 max_frequency +float32 amplitude +float32 duration \ No newline at end of file diff --git a/kortex_actuator_driver/msg/LoopSelection.msg b/kortex_actuator_driver/msg/LoopSelection.msg new file mode 100644 index 00000000..f8dc270b --- /dev/null +++ b/kortex_actuator_driver/msg/LoopSelection.msg @@ -0,0 +1,3 @@ + + +uint32 loop_selection \ No newline at end of file diff --git a/kortex_actuator_driver/msg/MessageId.msg b/kortex_actuator_driver/msg/MessageId.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_actuator_driver/msg/MessageId.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_actuator_driver/msg/NotificationHandle.msg b/kortex_actuator_driver/msg/NotificationHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_actuator_driver/msg/NotificationHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_actuator_driver/msg/NotificationOptions.msg b/kortex_actuator_driver/msg/NotificationOptions.msg new file mode 100644 index 00000000..6ff46842 --- /dev/null +++ b/kortex_actuator_driver/msg/NotificationOptions.msg @@ -0,0 +1,5 @@ + + +uint32 type +uint32 rate_m_sec +float32 threshold_value \ No newline at end of file diff --git a/kortex_actuator_driver/msg/NotificationType.msg b/kortex_actuator_driver/msg/NotificationType.msg new file mode 100644 index 00000000..79dd058c --- /dev/null +++ b/kortex_actuator_driver/msg/NotificationType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_NOTIFICATION_TYPE = 0 + +uint32 THRESHOLD = 1 + +uint32 FIX_RATE = 2 + +uint32 EVENT = 3 diff --git a/kortex_actuator_driver/msg/Permission.msg b/kortex_actuator_driver/msg/Permission.msg new file mode 100644 index 00000000..c5399e9c --- /dev/null +++ b/kortex_actuator_driver/msg/Permission.msg @@ -0,0 +1,8 @@ + +uint32 NO_PERMISSION = 0 + +uint32 READ_PERMISSION = 1 + +uint32 UPDATE_PERMISSION = 2 + +uint32 DELETE_PERMISSION = 4 diff --git a/kortex_actuator_driver/msg/PositionCommand.msg b/kortex_actuator_driver/msg/PositionCommand.msg new file mode 100644 index 00000000..5e894e42 --- /dev/null +++ b/kortex_actuator_driver/msg/PositionCommand.msg @@ -0,0 +1,4 @@ + +float32 position +float32 velocity +float32 acceleration \ No newline at end of file diff --git a/kortex_actuator_driver/msg/RampResponse.msg b/kortex_actuator_driver/msg/RampResponse.msg new file mode 100644 index 00000000..59d88d4e --- /dev/null +++ b/kortex_actuator_driver/msg/RampResponse.msg @@ -0,0 +1,6 @@ + + +uint32 loop_selection +float32 slope +float32 ramp_delay +float32 duration \ No newline at end of file diff --git a/kortex_actuator_driver/msg/SafetyHandle.msg b/kortex_actuator_driver/msg/SafetyHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_actuator_driver/msg/SafetyHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_actuator_driver/msg/SafetyIdentifier.msg b/kortex_actuator_driver/msg/SafetyIdentifier.msg new file mode 100644 index 00000000..6e0c3e8d --- /dev/null +++ b/kortex_actuator_driver/msg/SafetyIdentifier.msg @@ -0,0 +1,50 @@ + +uint32 UNSPECIFIED_ACTUATOR_SAFETY_IDENTIFIER = 0 + +uint32 FOLLOWING_ERROR = 1 + +uint32 MAXIMUM_VELOCITY = 2 + +uint32 JOINT_LIMIT_HIGH = 4 + +uint32 JOINT_LIMIT_LOW = 8 + +uint32 STRAIN_GAUGE_MISMATCH = 16 + +uint32 MAXIMUM_TORQUE = 32 + +uint32 UNRELIABLE_ABSOLUTE_POSITION = 64 + +uint32 MAGNETIC_POSITION = 128 + +uint32 HALL_POSITION = 256 + +uint32 HALL_SEQUENCE = 512 + +uint32 INPUT_ENCODER_HALL_MISMATCH = 1024 + +uint32 INPUT_ENCODER_INDEX_MISMATCH = 2048 + +uint32 INPUT_ENCODER_MAGNETIC_MISMATCH = 4096 + +uint32 MAXIMUM_MOTOR_CURRENT = 8192 + +uint32 MOTOR_CURRENT_MISMATCH = 16384 + +uint32 MAXIMUM_VOLTAGE = 32768 + +uint32 MINIMUM_VOLTAGE = 65536 + +uint32 MAXIMUM_MOTOR_TEMPERATURE = 131072 + +uint32 MAXIMUM_CORE_TEMPERATURE = 262144 + +uint32 NON_VOLATILE_MEMORY_CORRUPTED = 524288 + +uint32 MOTOR_DRIVER_FAULT = 1048576 + +uint32 EMERGENCY_LINE_ASSERTED = 2097152 + +uint32 COMMUNICATION_TICK_LOST = 4194304 + +uint32 WATCHDOG_TRIGGERED = 8388608 diff --git a/kortex_actuator_driver/msg/SafetyLimitType.msg b/kortex_actuator_driver/msg/SafetyLimitType.msg new file mode 100644 index 00000000..a8d7a177 --- /dev/null +++ b/kortex_actuator_driver/msg/SafetyLimitType.msg @@ -0,0 +1,4 @@ + +uint32 MAXIMAL_LIMIT = 0 + +uint32 MINIMAL_LIMIT = 1 diff --git a/kortex_actuator_driver/msg/SafetyNotification.msg b/kortex_actuator_driver/msg/SafetyNotification.msg new file mode 100644 index 00000000..36d88894 --- /dev/null +++ b/kortex_actuator_driver/msg/SafetyNotification.msg @@ -0,0 +1,11 @@ + + +SafetyHandle safety_handle + +uint32 value + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_actuator_driver/msg/SafetyStatusValue.msg b/kortex_actuator_driver/msg/SafetyStatusValue.msg new file mode 100644 index 00000000..2ea80554 --- /dev/null +++ b/kortex_actuator_driver/msg/SafetyStatusValue.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED = 0 + +uint32 WARNING = 1 + +uint32 ERROR = 2 + +uint32 NORMAL = 3 diff --git a/kortex_actuator_driver/msg/ServiceVersion.msg b/kortex_actuator_driver/msg/ServiceVersion.msg new file mode 100644 index 00000000..9665d1c2 --- /dev/null +++ b/kortex_actuator_driver/msg/ServiceVersion.msg @@ -0,0 +1,4 @@ + +uint32 RESERVED_0 = 0 + +uint32 CURRENT_VERSION = 1 diff --git a/kortex_actuator_driver/msg/Servoing.msg b/kortex_actuator_driver/msg/Servoing.msg new file mode 100644 index 00000000..7f2750b4 --- /dev/null +++ b/kortex_actuator_driver/msg/Servoing.msg @@ -0,0 +1,2 @@ + +bool enabled \ No newline at end of file diff --git a/kortex_actuator_driver/msg/StepResponse.msg b/kortex_actuator_driver/msg/StepResponse.msg new file mode 100644 index 00000000..bde4ed2e --- /dev/null +++ b/kortex_actuator_driver/msg/StepResponse.msg @@ -0,0 +1,6 @@ + + +uint32 loop_selection +float32 amplitude +float32 step_delay +float32 duration \ No newline at end of file diff --git a/kortex_actuator_driver/msg/Timestamp.msg b/kortex_actuator_driver/msg/Timestamp.msg new file mode 100644 index 00000000..5e60508c --- /dev/null +++ b/kortex_actuator_driver/msg/Timestamp.msg @@ -0,0 +1,3 @@ + +uint32 sec +uint32 usec \ No newline at end of file diff --git a/kortex_actuator_driver/msg/TorqueCalibration.msg b/kortex_actuator_driver/msg/TorqueCalibration.msg new file mode 100644 index 00000000..a237df13 --- /dev/null +++ b/kortex_actuator_driver/msg/TorqueCalibration.msg @@ -0,0 +1,5 @@ + +float32 global_gain +float32 global_offset +float32[] gain +float32[] offset \ No newline at end of file diff --git a/kortex_actuator_driver/msg/TorqueOffset.msg b/kortex_actuator_driver/msg/TorqueOffset.msg new file mode 100644 index 00000000..198518a2 --- /dev/null +++ b/kortex_actuator_driver/msg/TorqueOffset.msg @@ -0,0 +1,2 @@ + +float32 torque_offset \ No newline at end of file diff --git a/kortex_actuator_driver/msg/Unit.msg b/kortex_actuator_driver/msg/Unit.msg new file mode 100644 index 00000000..c019aeba --- /dev/null +++ b/kortex_actuator_driver/msg/Unit.msg @@ -0,0 +1,28 @@ + +uint32 UNSPECIFIED_UNIT = 0 + +uint32 CELSIUS = 1 + +uint32 AMPERE = 2 + +uint32 VOLT = 3 + +uint32 METER_PER_SECOND = 4 + +uint32 DEGREE_PER_SECOND = 5 + +uint32 METER_PER_SECOND_2 = 6 + +uint32 DEGREE_PER_SECOND_2 = 7 + +uint32 NEWTON = 8 + +uint32 NEWTON_METER = 9 + +uint32 KILOGRAM = 10 + +uint32 DEGREE = 11 + +uint32 TICK = 12 + +uint32 DEGREE_PER_MILLISECOND = 13 diff --git a/kortex_actuator_driver/msg/UserProfileHandle.msg b/kortex_actuator_driver/msg/UserProfileHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_actuator_driver/msg/UserProfileHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_actuator_driver/msg/VectorDriveParameters.msg b/kortex_actuator_driver/msg/VectorDriveParameters.msg new file mode 100644 index 00000000..8fad9fca --- /dev/null +++ b/kortex_actuator_driver/msg/VectorDriveParameters.msg @@ -0,0 +1,5 @@ + +float32 kpq +float32 kiq +float32 kpd +float32 kid \ No newline at end of file diff --git a/kortex_actuator_driver/msg/non_generated/ApiOptions.msg b/kortex_actuator_driver/msg/non_generated/ApiOptions.msg new file mode 100644 index 00000000..471fddd8 --- /dev/null +++ b/kortex_actuator_driver/msg/non_generated/ApiOptions.msg @@ -0,0 +1 @@ +uint32 timeout_ms \ No newline at end of file diff --git a/kortex_actuator_driver/msg/non_generated/CyclicStatus.msg b/kortex_actuator_driver/msg/non_generated/CyclicStatus.msg new file mode 100644 index 00000000..cb36958f --- /dev/null +++ b/kortex_actuator_driver/msg/non_generated/CyclicStatus.msg @@ -0,0 +1 @@ +bool isActive \ No newline at end of file diff --git a/kortex_actuator_driver/msg/non_generated/KortexError.msg b/kortex_actuator_driver/msg/non_generated/KortexError.msg new file mode 100644 index 00000000..3ff9c32c --- /dev/null +++ b/kortex_actuator_driver/msg/non_generated/KortexError.msg @@ -0,0 +1,3 @@ +uint32 code +uint32 subCode +string description \ No newline at end of file diff --git a/kortex_actuator_driver/package.xml b/kortex_actuator_driver/package.xml new file mode 100644 index 00000000..410d5568 --- /dev/null +++ b/kortex_actuator_driver/package.xml @@ -0,0 +1,35 @@ + + + kortex_actuator_driver + 1.0.0 + The kortex package that communicate with an actuator. + + + KINOVA + + + + + + BSD + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_generation + message_runtime + + + + + + + + diff --git a/kortex_actuator_driver/protos/ActuatorConfig.options b/kortex_actuator_driver/protos/ActuatorConfig.options new file mode 100644 index 00000000..ef52a309 --- /dev/null +++ b/kortex_actuator_driver/protos/ActuatorConfig.options @@ -0,0 +1,10 @@ +Kinova.Api.ActuatorConfig.TorqueCalibration.gain max_count:4 fixed_count:true +Kinova.Api.ActuatorConfig.TorqueCalibration.offset max_count:4 fixed_count:true +Kinova.Api.ActuatorConfig.ControlLoopParameters.kAz max_count:5 fixed_count:true +Kinova.Api.ActuatorConfig.ControlLoopParameters.kBz max_count:6 fixed_count:true +Kinova.Api.ActuatorConfig.CustomDataSelection.channel max_count:16 +Kinova.Api.ActuatorConfig.SafetyLimitType long_names:false +Kinova.Api.ActuatorConfig.ControlMode long_names:false +Kinova.Api.ActuatorConfig.CommandMode long_names:false +Kinova.Api.ActuatorConfig.SafetyIdentifier long_names:false +Kinova.Api.ActuatorConfig.ControlLoopSelection long_names:false diff --git a/kortex_actuator_driver/protos/ActuatorConfig.proto b/kortex_actuator_driver/protos/ActuatorConfig.proto new file mode 100644 index 00000000..8aca4183 --- /dev/null +++ b/kortex_actuator_driver/protos/ActuatorConfig.proto @@ -0,0 +1,235 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.ActuatorConfig; + +service ActuatorConfig {//@PROXY_ID=10 @ERROR=Kinova.Api.Error + rpc GetAxisOffsets (Kinova.Api.Common.Empty) returns (AxisOffsets); //@RPC_ID=1 + rpc SetAxisOffsets (AxisPosition) returns (Kinova.Api.Common.Empty); //@RPC_ID=2 + rpc ReadTorqueCalibration (Kinova.Api.Common.Empty) returns (TorqueCalibration); //@RPC_ID=3 + rpc WriteTorqueCalibration (TorqueCalibration) returns (Kinova.Api.Common.Empty); //@RPC_ID=4 + rpc SetTorqueOffset (TorqueOffset) returns (Kinova.Api.Common.Empty); //@RPC_ID=5 + rpc GetControlMode (Kinova.Api.Common.Empty) returns (ControlModeInformation); //@RPC_ID=6 + rpc SetControlMode (ControlModeInformation) returns (Kinova.Api.Common.Empty); //@RPC_ID=7 + rpc GetActivatedControlLoop (Kinova.Api.Common.Empty) returns (ControlLoop); //@RPC_ID=8 + rpc SetActivatedControlLoop (ControlLoop) returns (Kinova.Api.Common.Empty); //@RPC_ID=9 + rpc GetVectorDriveParameters (Kinova.Api.Common.Empty) returns (VectorDriveParameters); //@RPC_ID=10 + rpc SetVectorDriveParameters (VectorDriveParameters) returns (Kinova.Api.Common.Empty); //@RPC_ID=11 + rpc GetEncoderDerivativeParameters (Kinova.Api.Common.Empty) returns (EncoderDerivativeParameters); //@RPC_ID=12 + rpc SetEncoderDerivativeParameters (EncoderDerivativeParameters) returns (Kinova.Api.Common.Empty); //@RPC_ID=13 + rpc GetControlLoopParameters (LoopSelection) returns (ControlLoopParameters); //@RPC_ID=14 + rpc SetControlLoopParameters (ControlLoopParameters) returns (Kinova.Api.Common.Empty); //@RPC_ID=15 + rpc StartFrequencyResponse (FrequencyResponse) returns (Kinova.Api.Common.Empty); //@RPC_ID=16 + rpc StopFrequencyResponse (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=17 + rpc StartStepResponse (StepResponse) returns (Kinova.Api.Common.Empty); //@RPC_ID=18 + rpc StopStepResponse (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=19 + rpc StartRampResponse (RampResponse) returns (Kinova.Api.Common.Empty); //@RPC_ID=20 + rpc StopRampResponse (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=21 + rpc SelectCustomData (CustomDataSelection) returns (Kinova.Api.Common.Empty); //@RPC_ID=22 + rpc GetSelectedCustomData (Kinova.Api.Common.Empty) returns (CustomDataSelection); //@RPC_ID=23 + rpc SetCommandMode (CommandModeInformation) returns (Kinova.Api.Common.Empty); //@RPC_ID=24 + rpc ClearFaults (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=25 + rpc SetServoing (Servoing) returns (Kinova.Api.Common.Empty); //@RPC_ID=26 + rpc MoveToPosition (PositionCommand) returns (Kinova.Api.Common.Empty); //@RPC_ID=27 + rpc GetCommandMode (Kinova.Api.Common.Empty) returns (CommandModeInformation); //@RPC_ID=28 + rpc GetServoing (Kinova.Api.Common.Empty) returns (Servoing); //@RPC_ID=29 + rpc GetTorqueOffset (Kinova.Api.Common.Empty) returns (TorqueOffset); //@RPC_ID=30 +} + +enum ServiceVersion +{ + RESERVED_0 = 0; // Reserved + CURRENT_VERSION = 1; // Current Version +} + +/** + * list of limit types + */ +enum SafetyLimitType +{ + MAXIMAL_LIMIT = 0; // Maximal limit + MINIMAL_LIMIT = 1; // Minimal limit +} + +/** + * list of control modes + */ +enum ControlMode +{ + NONE = 0; // None + POSITION = 1; // Position mode + VELOCITY = 2; // Velocity mode + TORQUE = 3; // Torque mode + CURRENT = 4; // Current mode + CUSTOM = 5; // Custom mode +} + +/** + * list of command modes + */ +enum CommandMode +{ + CYCLIC = 0; // Only cyclic data + ASYNC_CYCLIC_FLAGS = 1; // Not Supported + ASYNC = 2; // Only asynchronous messages + CYCLIC_JITTERCOMPENSATED_POSITION = 3; // Smoothing using only position inputs + CYCLIC_JITTERCOMPENSATED_VELOCITY = 4; // Smoothing using position and velocity inputs + CYCLIC_JITTERCOMPENSATED_ACCELERATION = 5; // Smoothing using position, velocity and acceleration inputs (not supported) +} + +enum ControlLoopSelection +{ + RESERVED = 0; + JOINT_POSITION = 1; // If available + MOTOR_POSITION = 2; + JOINT_VELOCITY = 4; // If available + MOTOR_VELOCITY = 8; + JOINT_TORQUE = 16; + MOTOR_CURRENT = 32; +} + +message AxisPosition +{ + float position = 1; // Axis position +} + +message AxisOffsets +{ + float absolute_offset = 1; // Absolute offset value + float relative_offset = 2; // Relative offset value +} + +message TorqueCalibration +{ + float global_gain = 1; // Global gain value + float global_offset = 2; // Global offset value + repeated float gain = 3; // Gain (index 0 to 3) + repeated float offset = 4; // Offset (index 0 to 3) +} + +message TorqueOffset +{ + float torque_offset = 1; // Torque offset value +} + +message ControlModeInformation +{ + ControlMode control_mode = 1; // Control mode +} + +/* Control loop bit mask : + Bit 0 : JOINT_POSITION // If available + Bit 1 : MOTOR_POSITION + Bit 2 : JOINT_VELOCITY // If available + Bit 3 : MOTOR_VELOCITY + Bit 4 : JOINT_TORQUE + Bit 5 : MOTOR_CURRENT + */ +message ControlLoop +{ + fixed32 control_loop = 1; // Use ControlLoopSelection enum values to form bitmask +} + +message LoopSelection +{ + ControlLoopSelection loop_selection = 1; // ControlLoopSelection enum +} + +message VectorDriveParameters +{ + float kpq = 1; // Kpq + float kiq = 2; // Kiq + float kpd = 3; // Kpd + float kid = 4; // Kid +} + +message EncoderDerivativeParameters +{ + uint32 max_window_width = 1; // Maximum window width + uint32 min_encoder_tick_count = 2; // Minimum encoder tick count +} + +message ControlLoopParameters +{ + ControlLoopSelection loop_selection = 1; // ControlLoopSelection enum + float error_saturation = 2; // Error saturation value + float output_saturation = 3; // Output saturation value + repeated float kAz = 4; // KAz (index 0 to 4) + repeated float kBz = 5; // KBz (index 0 to 5) +} + +message FrequencyResponse +{ + ControlLoopSelection loop_selection = 1; // ControlLoopSelection enum + float min_frequency = 2; // Minimum frequency value + float max_frequency = 3; // Maximum frequency value + float amplitude = 4; // Amplitude value + float duration = 5; // Duration in seconds +} + +message StepResponse +{ + ControlLoopSelection loop_selection = 1; // ControlLoopSelection enum + float amplitude = 2; // Amplitude value + float step_delay = 3; // Step delay value + float duration = 4; // Duration in seconds +} + +message RampResponse +{ + ControlLoopSelection loop_selection = 1; // ControlLoopSelection enum + float slope = 2; // Slope value + float ramp_delay = 3; // Ramp delay value + float duration = 4; // Duration in seconds +} + +message CustomDataSelection +{ + repeated uint32 channel = 1; // 16 channels maximum +} + +message CommandModeInformation +{ + CommandMode command_mode = 1; // Command mode +} + +message Servoing +{ + bool enabled = 1; // Servoing enabled +} + +message PositionCommand +{ + float position = 1; // Position value + float velocity = 2; // Velocity value + float acceleration = 3; // Acceleration value +} + +enum SafetyIdentifier { + UNSPECIFIED_ACTUATOR_SAFETY_IDENTIFIER = 0; //0x0 + FOLLOWING_ERROR = 1; //0x1 + MAXIMUM_VELOCITY = 2; //0x2 + JOINT_LIMIT_HIGH = 4; //0x4 + JOINT_LIMIT_LOW = 8; //0x8 + STRAIN_GAUGE_MISMATCH = 16; //0x10 + MAXIMUM_TORQUE = 32; //0x20 + UNRELIABLE_ABSOLUTE_POSITION = 64; //0x40 + MAGNETIC_POSITION = 128; //0x80 + HALL_POSITION = 256; //0x100 + HALL_SEQUENCE = 512; //0x200 + INPUT_ENCODER_HALL_MISMATCH = 1024; //0x400 + INPUT_ENCODER_INDEX_MISMATCH = 2048; //0x800 + INPUT_ENCODER_MAGNETIC_MISMATCH = 4096; //0x1000 + MAXIMUM_MOTOR_CURRENT = 8192; //0x2000 + MOTOR_CURRENT_MISMATCH = 16384; //0x4000 + MAXIMUM_VOLTAGE = 32768; //0x8000 + MINIMUM_VOLTAGE = 65536; //0x10000 + MAXIMUM_MOTOR_TEMPERATURE = 131072; //0x20000 + MAXIMUM_CORE_TEMPERATURE = 262144; //0x40000 + NON_VOLATILE_MEMORY_CORRUPTED = 524288; //0x80000 + MOTOR_DRIVER_FAULT = 1048576; //0x100000 + EMERGENCY_LINE_ASSERTED = 2097152; //0x200000 + COMMUNICATION_TICK_LOST = 4194304; //0x400000 + WATCHDOG_TRIGGERED = 8388608; //0x800000 +} diff --git a/kortex_actuator_driver/protos/ActuatorCyclic.proto b/kortex_actuator_driver/protos/ActuatorCyclic.proto new file mode 100644 index 00000000..5508da2b --- /dev/null +++ b/kortex_actuator_driver/protos/ActuatorCyclic.proto @@ -0,0 +1,78 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.ActuatorCyclic; + +/** + * List of services available via remote procedure call + */ +service ActuatorCyclic {//@PROXY_ID=11 @ERROR=Kinova.Api.Error + +// Commands refresh (with feedback) + rpc Refresh (Command) returns (Feedback); //@RPC_ID=1 + +// Commands refresh (no feedback) + rpc RefreshCommand (Command) returns (Kinova.Api.Common.Empty); //@RPC_ID=2 + +// Gets feedback + rpc RefreshFeedback (MessageId) returns (Feedback); //@RPC_ID=3 + +// Gets custom data + rpc RefreshCustomData (MessageId) returns (CustomData); //@RPC_ID=4 +} + +enum ServiceVersion { + RESERVED_0 = 0; // Reserved + CURRENT_VERSION = 1; // Current Service Version +} + +message MessageId { + fixed32 identifier = 1; // Message ID (first 2 bytes : device ID, last 2 bytes : sequence number) +} + +message Command { + MessageId command_id = 1; // MessageId + fixed32 flags = 2; // Flags + float position = 3; // Desired position of the actuator in ° + float velocity = 4; // Desired velocity of the actuator in °/s + float torque_joint = 5; // Desired torque of the actuator in N·m + float current_motor = 6; // Desired current of the motor in A +} + +message Feedback { + MessageId feedback_id = 1; // MessageId + fixed32 status_flags = 2; // Status flags + fixed32 jitter_comm = 3; // Jitter from the communication in μs + float position = 4; // Position of the actuator in ° + float velocity = 5; // Angular velocity of the actuator in °/s + float torque = 6; // Torque of the actuator in N·m + float current_motor = 7; // Current of the motor in A + float voltage = 8; // Voltage of the main board in V + float temperature_motor = 9; // Motor temperature (average of the three (3) temperatures in °C) + float temperature_core = 10; // Microcontroller temperature in °C + fixed32 fault_bank_a = 11; // Bank A Fault (see ActuatorConfig.SafetyIdentifier) + fixed32 fault_bank_b = 12; // Bank B Fault (see ActuatorConfig.SafetyIdentifier) + fixed32 warning_bank_a = 13; // Bank A Warning (see ActuatorConfig.SafetyIdentifier) + fixed32 warning_bank_b = 14; // Bank B Warning (see ActuatorConfig.SafetyIdentifier) +} + +message CustomData { + MessageId custom_data_id = 1; // MessageId + fixed32 custom_data_0 = 2; // Custom data word 0 + fixed32 custom_data_1 = 3; // Custom data word 1 + fixed32 custom_data_2 = 4; // Custom data word 2 + fixed32 custom_data_3 = 5; // Custom data word 3 + fixed32 custom_data_4 = 6; // Custom data word 4 + fixed32 custom_data_5 = 7; // Custom data word 5 + fixed32 custom_data_6 = 8; // Custom data word 6 + fixed32 custom_data_7 = 9; // Custom data word 7 + fixed32 custom_data_8 = 10; // Custom data word 8 + fixed32 custom_data_9 = 11; // Custom data word 9 + fixed32 custom_data_10 = 12; // Custom data word 10 + fixed32 custom_data_11 = 13; // Custom data word 11 + fixed32 custom_data_12 = 14; // Custom data word 12 + fixed32 custom_data_13 = 15; // Custom data word 13 + fixed32 custom_data_14 = 16; // Custom data word 14 + fixed32 custom_data_15 = 17; // Custom data word 15 +} diff --git a/kortex_actuator_driver/protos/Common.options b/kortex_actuator_driver/protos/Common.options new file mode 100644 index 00000000..5f3020ab --- /dev/null +++ b/kortex_actuator_driver/protos/Common.options @@ -0,0 +1,4 @@ +Kinova.Api.Common.DeviceTypes long_names:false +Kinova.Api.Common.SafetyStatusValue long_names:false +Kinova.Api.Common.NotificationType long_names:false +Kinova.Api.Common.Unit long_names:false \ No newline at end of file diff --git a/kortex_actuator_driver/protos/Common.proto b/kortex_actuator_driver/protos/Common.proto new file mode 100644 index 00000000..50f762ec --- /dev/null +++ b/kortex_actuator_driver/protos/Common.proto @@ -0,0 +1,152 @@ +syntax = "proto3"; + +package Kinova.Api.Common; + +/** + * list of possible device types + */ +enum DeviceTypes { + UNSPECIFIED_DEVICE_TYPE = 0; + BASE = 1; + VISION = 2; + BIG_ACTUATOR = 3; + SMALL_ACTUATOR = 4; + INTERCONNECT = 5; + GRIPPER = 6; +} + +/** +* Message contains information about a device - device type, device identifier, and the order of the device within the robot +*/ +message DeviceHandle { + DeviceTypes device_type = 1; + uint32 device_identifier = 2; // Unique device identifier (used with other services) + uint32 order = 3; // Unique value indicating the order of that device versus the others to facilitate representation +} + +/** + * list of possible safety statuses + */ +enum SafetyStatusValue { + UNSPECIFIED = 0; + WARNING = 1; //warning safety reached + ERROR = 2; //error safety reached + NORMAL = 3; //safety is off +} + +/** + * Enumeration used as bitfields wih permission field + */ +enum Permission { + NO_PERMISSION = 0; + READ_PERMISSION = 1; //refers to a user's capability to read the entity + UPDATE_PERMISSION = 2;//refers to a user's capability to write or modify the entity + DELETE_PERMISSION = 4; //refers to a user's capability to delete the entity +} + +/** + * list of notification types + */ +enum NotificationType { + UNSPECIFIED_NOTIFICATION_TYPE = 0; + THRESHOLD = 1; + FIX_RATE = 2; + EVENT = 3; //Event type. Only this one is supported for now +} + +/** + * list of units used throughout API methods + */ +enum Unit { + UNSPECIFIED_UNIT = 0; + CELSIUS = 1; + AMPERE = 2; + VOLT = 3; + METER_PER_SECOND = 4; + DEGREE_PER_SECOND = 5; + METER_PER_SECOND_2 = 6; + DEGREE_PER_SECOND_2 = 7; + NEWTON = 8; + NEWTON_METER =9; + KILOGRAM = 10; + DEGREE = 11; + TICK = 12; + DEGREE_PER_MILLISECOND = 13; +} + +/** + * Message used when no information needs to be exchanged between client application and robot, and vice versa + */ +message Empty { +} + +/** + * Notification options + */ +message NotificationOptions { + NotificationType type = 1; //type of notification + uint32 rate_m_sec = 2; + float threshold_value = 3; +} + +/** + * Handle to a safety + */ +message SafetyHandle { + uint32 identifier = 1; +} + +/** + * Handle to a notification + */ +message NotificationHandle { + uint32 identifier = 1; +} + +/** + * Message that contains a Safety event + */ +message SafetyNotification { + SafetyHandle safety_handle = 1; //safety handle + SafetyStatusValue value = 2; //new safety status + Timestamp timestamp = 3; //event timestamp + UserProfileHandle user_handle = 4; //user that caused the safety event + Connection connection = 5; // connection that caused the safety event +} + +/** + * Timestamp based on epoch + */ +message Timestamp { + uint32 sec = 1; //epoch in seconds since 1970 + uint32 usec = 2;//microseconds after the second (0-999999) +} + +/** + * Handle to an existing User Profile. + */ +message UserProfileHandle { + uint32 identifier = 1; //User profile identifier + fixed32 permission = 2; //must use 'Permission' as bitwise +} + +message Connection { + UserProfileHandle user_handle = 1; //user profile handle, or set to zero if no user logged in + string connection_information = 2; //connection info (ex. IP address with port number) + uint32 connection_identifier = 3; //connection identifier +} + +enum ArmState +{ + UNSPECIFIED_ARM_STATE = 0; + BASE_INITIALIZATION = 1; // Cannot be reported as the Base initialization must be completed before allowing user connection + IDLE = 2; + ARM_INITIALIZATION = 3; + ARM_IN_FAULT = 4; + ARM_MAINTENANCE = 5; + ARM_SERVOING_LOW_LEVEL = 6; + ARM_SERVOING_READY = 7; + ARM_SERVOING_PLAYING_SEQUENCE = 8; + ARM_SERVOING_MANUALLY_CONTROLLED = 9; + RESERVED = 255; // For debugging, this state must never be reported outside the base. this means that a state is not mapped correctly +} \ No newline at end of file diff --git a/kortex_actuator_driver/readme.md b/kortex_actuator_driver/readme.md new file mode 100644 index 00000000..8b503711 --- /dev/null +++ b/kortex_actuator_driver/readme.md @@ -0,0 +1,121 @@ + + +# Kortex actuator driver +This package is a node that interfaces with a single actuator. + + + +1. [Content](#content) + 1. [build](#build) + 1. [msg](#msg) + 1. [non_generated](#non_generated) + 1. [protos](#protos) + 1. [src](#src) + 1. [srv](#srv) + 1. [non_generated](#non_generated-1) + 1. [templates](#templates) +1. [How to start the node](#how-to-start-the-node) + 1. [Normal mode](#normal-mode) + 1. [Using the device routing feature](#using-the-device-routing-feature) +1. [Generation](#generation) + + + + +## Content + +### build +This folder's only purpose is to exist as a temp folder during the generation. It should not be used. + +### msg +This folder contains every custom messages used by the node **kortex\_actuator\_driver**. All the .msg files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .msg files used by the node **kortex\_actuator\_driver**. + +| MSG | Description | +|:---:|:---:| +| ApiOptions.msg | A set of option that is supported by the Kortex API. It is used with the service SetApiOptions. For now there is only one option called tiemout_ms and it lets the user set a timeout value on every next service call. | +| KortexError.msg | Describe the topic /KortexError. Every service call of the node kortex_actuator_driver will publish in /KortexError everytime the Kortex API returns an error. | + + +### protos +This folder contains the protobuf files from where the MSG, SRV and sources files are generated. The content of this folder should not be modified. + +### src +This folder contains all the generated source files needed to build the node. The content of this folder should not be modified. + +### srv +This folder contains every custom services used by the node **kortex\_actuator\_driver**. All the .srv files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .srv files used by the node **kortex\_actuator\_driver**. + +| SRV | Description | +|:---:|:---:| +| SetApiOptions.srv | It modify the api options of the Kortex API. Once this service is called, the options set will affect every future call to the node. | +| SetDeviceID.srv | It modify the target device (device routing feature) of the node. The default value is 0.| + + +### templates +This folder contains all the JINJA2 files needed by the protoc generator. For more details on the generation process, see the [Generation](#generation) section. + +| JINJA2 files | Description | +|:---:|:---:| +| main.jinja2 | Use to generate src/main.cpp | +| NodeServices.cpp.jinja2 | Use to generate src/node.cpp | +| NodeServices.h.jinja2 | Use to generate src/node.h | +| proto_converterCPP.jinja2 | Use to generate every src/*_proto\_converter.cpp files | +| proto_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_converterCPP.jinja2 | Use to generate every src/*_ros\_converter.cpp files | +| ros_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_enum.jinja2 | Use to generate every msg/*.msg files that represent a protobuf enum | +| ros_message.jinja2 | Use to generate every msg/*.msg files that represent a protobuf message | +| ros_oneof.jinja2 | Use to generate every msg/*.msg files that represent a protobuf oneof | +| ros_service.jinja2 | Use to generate every msg/*.msg files that represent a protobuf RPC | + + +## How to start the node + +### Normal mode +Use this mode if you are directly (there is an ethernet cable between your computer and an actuator) connected to an actuator. To be clear, if you are using the WIFI or your cable is connected in the robot's base, don't use this mode. + +rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100 + +In the command above, you would be running the kortex_actuator_driver node on an actuator with an IP address of 192.168.1.10. The cyclic data would be refreshed at 100 Hz. + + +### Using the device routing feature +Use this mode if your are connected via WIFI are directly in the robot's base and you want to have access to an actuator's interface. + +rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100 4 + +In the command above, you would be running the kortex_actuator_driver node using the device routing feature on an robot's base with an IP address of 192.168.1.10. The cyclic data would be refreshed at 100 Hz and the actuator's device ID is 4. + + +## Generation +

+The generation process is based on a custom protobuf plugin. Basically, most of the generation process is in the python file RosGeneration.py located at the package's root folder. Before launching the generation make sure you have the python module Jinja2 installed on your computer. +

+ +To launch the generation of this package: + +1. Open a terminal window. +1. Browse the root of this package [YOUR\_ROS\_WORKSPACE]/src/ros\_kortex/kortex\_actuator\_driver/ +1. Make sure that the file kortex_actuator_driver.sh can be executed. If not then chmod +x kortex_actuator_driver.sh +1. Run this command: protoc --plugin=protoc-gen-custom=kortex_actuator_driver.sh -I./protos/ --custom_out=./build ./protos/\*.prot +1. The result of the generation should be on thos folder: + * /src + * /msg + * /srv + diff --git a/kortex_actuator_driver/src/actuatorconfig_proto_converter.cpp b/kortex_actuator_driver/src/actuatorconfig_proto_converter.cpp new file mode 100644 index 00000000..1c7a4f74 --- /dev/null +++ b/kortex_actuator_driver/src/actuatorconfig_proto_converter.cpp @@ -0,0 +1,172 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "actuatorconfig_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_actuator_driver::AxisPosition input, AxisPosition *output) +{ + output->set_position(input.position); + + return 0; +} +int ToProtoData(kortex_actuator_driver::AxisOffsets input, AxisOffsets *output) +{ + output->set_absolute_offset(input.absolute_offset); + output->set_relative_offset(input.relative_offset); + + return 0; +} +int ToProtoData(kortex_actuator_driver::TorqueCalibration input, TorqueCalibration *output) +{ + output->set_global_gain(input.global_gain); + output->set_global_offset(input.global_offset); + output->clear_gain(); + for(int i = 0; i < input.gain.size(); i++) + { + output->add_gain(input.gain[i]); + } + + output->clear_offset(); + for(int i = 0; i < input.offset.size(); i++) + { + output->add_offset(input.offset[i]); + } + + + return 0; +} +int ToProtoData(kortex_actuator_driver::TorqueOffset input, TorqueOffset *output) +{ + output->set_torque_offset(input.torque_offset); + + return 0; +} +int ToProtoData(kortex_actuator_driver::ControlModeInformation input, ControlModeInformation *output) +{ + output->set_control_mode((Kinova::Api::ActuatorConfig::ControlMode)input.control_mode); + + return 0; +} +int ToProtoData(kortex_actuator_driver::ControlLoop input, ControlLoop *output) +{ + output->set_control_loop(input.control_loop); + + return 0; +} +int ToProtoData(kortex_actuator_driver::LoopSelection input, LoopSelection *output) +{ + output->set_loop_selection((Kinova::Api::ActuatorConfig::ControlLoopSelection)input.loop_selection); + + return 0; +} +int ToProtoData(kortex_actuator_driver::VectorDriveParameters input, VectorDriveParameters *output) +{ + output->set_kpq(input.kpq); + output->set_kiq(input.kiq); + output->set_kpd(input.kpd); + output->set_kid(input.kid); + + return 0; +} +int ToProtoData(kortex_actuator_driver::EncoderDerivativeParameters input, EncoderDerivativeParameters *output) +{ + output->set_max_window_width(input.max_window_width); + output->set_min_encoder_tick_count(input.min_encoder_tick_count); + + return 0; +} +int ToProtoData(kortex_actuator_driver::ControlLoopParameters input, ControlLoopParameters *output) +{ + output->set_loop_selection((Kinova::Api::ActuatorConfig::ControlLoopSelection)input.loop_selection); + output->set_error_saturation(input.error_saturation); + output->set_output_saturation(input.output_saturation); + output->clear_kaz(); + for(int i = 0; i < input.kAz.size(); i++) + { + output->add_kaz(input.kAz[i]); + } + + output->clear_kbz(); + for(int i = 0; i < input.kBz.size(); i++) + { + output->add_kbz(input.kBz[i]); + } + + + return 0; +} +int ToProtoData(kortex_actuator_driver::FrequencyResponse input, FrequencyResponse *output) +{ + output->set_loop_selection((Kinova::Api::ActuatorConfig::ControlLoopSelection)input.loop_selection); + output->set_min_frequency(input.min_frequency); + output->set_max_frequency(input.max_frequency); + output->set_amplitude(input.amplitude); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_actuator_driver::StepResponse input, StepResponse *output) +{ + output->set_loop_selection((Kinova::Api::ActuatorConfig::ControlLoopSelection)input.loop_selection); + output->set_amplitude(input.amplitude); + output->set_step_delay(input.step_delay); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_actuator_driver::RampResponse input, RampResponse *output) +{ + output->set_loop_selection((Kinova::Api::ActuatorConfig::ControlLoopSelection)input.loop_selection); + output->set_slope(input.slope); + output->set_ramp_delay(input.ramp_delay); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_actuator_driver::CustomDataSelection input, CustomDataSelection *output) +{ + output->clear_channel(); + for(int i = 0; i < input.channel.size(); i++) + { + output->add_channel(input.channel[i]); + } + + + return 0; +} +int ToProtoData(kortex_actuator_driver::CommandModeInformation input, CommandModeInformation *output) +{ + output->set_command_mode((Kinova::Api::ActuatorConfig::CommandMode)input.command_mode); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Servoing input, Servoing *output) +{ + output->set_enabled(input.enabled); + + return 0; +} +int ToProtoData(kortex_actuator_driver::PositionCommand input, PositionCommand *output) +{ + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_acceleration(input.acceleration); + + return 0; +} diff --git a/kortex_actuator_driver/src/actuatorconfig_proto_converter.h b/kortex_actuator_driver/src/actuatorconfig_proto_converter.h new file mode 100644 index 00000000..78ffe9f7 --- /dev/null +++ b/kortex_actuator_driver/src/actuatorconfig_proto_converter.h @@ -0,0 +1,82 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ActuatorConfigPROTO_CONVERTER_H_ +#define _KORTEX_ActuatorConfigPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/AxisPosition.h" +#include "kortex_actuator_driver/AxisOffsets.h" +#include "kortex_actuator_driver/TorqueCalibration.h" +#include "kortex_actuator_driver/TorqueOffset.h" +#include "kortex_actuator_driver/ControlModeInformation.h" +#include "kortex_actuator_driver/ControlLoop.h" +#include "kortex_actuator_driver/LoopSelection.h" +#include "kortex_actuator_driver/VectorDriveParameters.h" +#include "kortex_actuator_driver/EncoderDerivativeParameters.h" +#include "kortex_actuator_driver/ControlLoopParameters.h" +#include "kortex_actuator_driver/FrequencyResponse.h" +#include "kortex_actuator_driver/StepResponse.h" +#include "kortex_actuator_driver/RampResponse.h" +#include "kortex_actuator_driver/CustomDataSelection.h" +#include "kortex_actuator_driver/CommandModeInformation.h" +#include "kortex_actuator_driver/Servoing.h" +#include "kortex_actuator_driver/PositionCommand.h" + + +using namespace Kinova::Api::ActuatorConfig; + +int ToProtoData(kortex_actuator_driver::AxisPosition intput, AxisPosition *output); +int ToProtoData(kortex_actuator_driver::AxisOffsets intput, AxisOffsets *output); +int ToProtoData(kortex_actuator_driver::TorqueCalibration intput, TorqueCalibration *output); +int ToProtoData(kortex_actuator_driver::TorqueOffset intput, TorqueOffset *output); +int ToProtoData(kortex_actuator_driver::ControlModeInformation intput, ControlModeInformation *output); +int ToProtoData(kortex_actuator_driver::ControlLoop intput, ControlLoop *output); +int ToProtoData(kortex_actuator_driver::LoopSelection intput, LoopSelection *output); +int ToProtoData(kortex_actuator_driver::VectorDriveParameters intput, VectorDriveParameters *output); +int ToProtoData(kortex_actuator_driver::EncoderDerivativeParameters intput, EncoderDerivativeParameters *output); +int ToProtoData(kortex_actuator_driver::ControlLoopParameters intput, ControlLoopParameters *output); +int ToProtoData(kortex_actuator_driver::FrequencyResponse intput, FrequencyResponse *output); +int ToProtoData(kortex_actuator_driver::StepResponse intput, StepResponse *output); +int ToProtoData(kortex_actuator_driver::RampResponse intput, RampResponse *output); +int ToProtoData(kortex_actuator_driver::CustomDataSelection intput, CustomDataSelection *output); +int ToProtoData(kortex_actuator_driver::CommandModeInformation intput, CommandModeInformation *output); +int ToProtoData(kortex_actuator_driver::Servoing intput, Servoing *output); +int ToProtoData(kortex_actuator_driver::PositionCommand intput, PositionCommand *output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/actuatorconfig_ros_converter.cpp b/kortex_actuator_driver/src/actuatorconfig_ros_converter.cpp new file mode 100644 index 00000000..f9c7698e --- /dev/null +++ b/kortex_actuator_driver/src/actuatorconfig_ros_converter.cpp @@ -0,0 +1,172 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "actuatorconfig_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(AxisPosition input, kortex_actuator_driver::AxisPosition &output) +{ + output.position = input.position(); + + return 0; +} +int ToRosData(AxisOffsets input, kortex_actuator_driver::AxisOffsets &output) +{ + output.absolute_offset = input.absolute_offset(); + output.relative_offset = input.relative_offset(); + + return 0; +} +int ToRosData(TorqueCalibration input, kortex_actuator_driver::TorqueCalibration &output) +{ + output.global_gain = input.global_gain(); + output.global_offset = input.global_offset(); + + output.gain.clear(); + for(int i = 0; i < input.gain_size(); i++) + { + output.gain.push_back(input.gain(i)); + } + + output.offset.clear(); + for(int i = 0; i < input.offset_size(); i++) + { + output.offset.push_back(input.offset(i)); + } + + return 0; +} +int ToRosData(TorqueOffset input, kortex_actuator_driver::TorqueOffset &output) +{ + output.torque_offset = input.torque_offset(); + + return 0; +} +int ToRosData(ControlModeInformation input, kortex_actuator_driver::ControlModeInformation &output) +{ + output.control_mode = input.control_mode(); + + return 0; +} +int ToRosData(ControlLoop input, kortex_actuator_driver::ControlLoop &output) +{ + output.control_loop = input.control_loop(); + + return 0; +} +int ToRosData(LoopSelection input, kortex_actuator_driver::LoopSelection &output) +{ + output.loop_selection = input.loop_selection(); + + return 0; +} +int ToRosData(VectorDriveParameters input, kortex_actuator_driver::VectorDriveParameters &output) +{ + output.kpq = input.kpq(); + output.kiq = input.kiq(); + output.kpd = input.kpd(); + output.kid = input.kid(); + + return 0; +} +int ToRosData(EncoderDerivativeParameters input, kortex_actuator_driver::EncoderDerivativeParameters &output) +{ + output.max_window_width = input.max_window_width(); + output.min_encoder_tick_count = input.min_encoder_tick_count(); + + return 0; +} +int ToRosData(ControlLoopParameters input, kortex_actuator_driver::ControlLoopParameters &output) +{ + output.loop_selection = input.loop_selection(); + output.error_saturation = input.error_saturation(); + output.output_saturation = input.output_saturation(); + + output.kAz.clear(); + for(int i = 0; i < input.kaz_size(); i++) + { + output.kAz.push_back(input.kaz(i)); + } + + output.kBz.clear(); + for(int i = 0; i < input.kbz_size(); i++) + { + output.kBz.push_back(input.kbz(i)); + } + + return 0; +} +int ToRosData(FrequencyResponse input, kortex_actuator_driver::FrequencyResponse &output) +{ + output.loop_selection = input.loop_selection(); + output.min_frequency = input.min_frequency(); + output.max_frequency = input.max_frequency(); + output.amplitude = input.amplitude(); + output.duration = input.duration(); + + return 0; +} +int ToRosData(StepResponse input, kortex_actuator_driver::StepResponse &output) +{ + output.loop_selection = input.loop_selection(); + output.amplitude = input.amplitude(); + output.step_delay = input.step_delay(); + output.duration = input.duration(); + + return 0; +} +int ToRosData(RampResponse input, kortex_actuator_driver::RampResponse &output) +{ + output.loop_selection = input.loop_selection(); + output.slope = input.slope(); + output.ramp_delay = input.ramp_delay(); + output.duration = input.duration(); + + return 0; +} +int ToRosData(CustomDataSelection input, kortex_actuator_driver::CustomDataSelection &output) +{ + + output.channel.clear(); + for(int i = 0; i < input.channel_size(); i++) + { + output.channel.push_back(input.channel(i)); + } + + return 0; +} +int ToRosData(CommandModeInformation input, kortex_actuator_driver::CommandModeInformation &output) +{ + output.command_mode = input.command_mode(); + + return 0; +} +int ToRosData(Servoing input, kortex_actuator_driver::Servoing &output) +{ + output.enabled = input.enabled(); + + return 0; +} +int ToRosData(PositionCommand input, kortex_actuator_driver::PositionCommand &output) +{ + output.position = input.position(); + output.velocity = input.velocity(); + output.acceleration = input.acceleration(); + + return 0; +} diff --git a/kortex_actuator_driver/src/actuatorconfig_ros_converter.h b/kortex_actuator_driver/src/actuatorconfig_ros_converter.h new file mode 100644 index 00000000..39f37707 --- /dev/null +++ b/kortex_actuator_driver/src/actuatorconfig_ros_converter.h @@ -0,0 +1,82 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ActuatorConfigROS_CONVERTER_H_ +#define _KORTEX_ActuatorConfigROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/AxisPosition.h" +#include "kortex_actuator_driver/AxisOffsets.h" +#include "kortex_actuator_driver/TorqueCalibration.h" +#include "kortex_actuator_driver/TorqueOffset.h" +#include "kortex_actuator_driver/ControlModeInformation.h" +#include "kortex_actuator_driver/ControlLoop.h" +#include "kortex_actuator_driver/LoopSelection.h" +#include "kortex_actuator_driver/VectorDriveParameters.h" +#include "kortex_actuator_driver/EncoderDerivativeParameters.h" +#include "kortex_actuator_driver/ControlLoopParameters.h" +#include "kortex_actuator_driver/FrequencyResponse.h" +#include "kortex_actuator_driver/StepResponse.h" +#include "kortex_actuator_driver/RampResponse.h" +#include "kortex_actuator_driver/CustomDataSelection.h" +#include "kortex_actuator_driver/CommandModeInformation.h" +#include "kortex_actuator_driver/Servoing.h" +#include "kortex_actuator_driver/PositionCommand.h" + + +using namespace Kinova::Api::ActuatorConfig; + +int ToRosData(AxisPosition input, kortex_actuator_driver::AxisPosition &output); +int ToRosData(AxisOffsets input, kortex_actuator_driver::AxisOffsets &output); +int ToRosData(TorqueCalibration input, kortex_actuator_driver::TorqueCalibration &output); +int ToRosData(TorqueOffset input, kortex_actuator_driver::TorqueOffset &output); +int ToRosData(ControlModeInformation input, kortex_actuator_driver::ControlModeInformation &output); +int ToRosData(ControlLoop input, kortex_actuator_driver::ControlLoop &output); +int ToRosData(LoopSelection input, kortex_actuator_driver::LoopSelection &output); +int ToRosData(VectorDriveParameters input, kortex_actuator_driver::VectorDriveParameters &output); +int ToRosData(EncoderDerivativeParameters input, kortex_actuator_driver::EncoderDerivativeParameters &output); +int ToRosData(ControlLoopParameters input, kortex_actuator_driver::ControlLoopParameters &output); +int ToRosData(FrequencyResponse input, kortex_actuator_driver::FrequencyResponse &output); +int ToRosData(StepResponse input, kortex_actuator_driver::StepResponse &output); +int ToRosData(RampResponse input, kortex_actuator_driver::RampResponse &output); +int ToRosData(CustomDataSelection input, kortex_actuator_driver::CustomDataSelection &output); +int ToRosData(CommandModeInformation input, kortex_actuator_driver::CommandModeInformation &output); +int ToRosData(Servoing input, kortex_actuator_driver::Servoing &output); +int ToRosData(PositionCommand input, kortex_actuator_driver::PositionCommand &output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/actuatorcyclic_proto_converter.cpp b/kortex_actuator_driver/src/actuatorcyclic_proto_converter.cpp new file mode 100644 index 00000000..dbf2601d --- /dev/null +++ b/kortex_actuator_driver/src/actuatorcyclic_proto_converter.cpp @@ -0,0 +1,79 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "actuatorcyclic_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_actuator_driver::MessageId input, MessageId *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Command input, Command *output) +{ + ToProtoData(input.command_id, output->mutable_command_id()); + output->set_flags(input.flags); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_torque_joint(input.torque_joint); + output->set_current_motor(input.current_motor); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Feedback input, Feedback *output) +{ + ToProtoData(input.feedback_id, output->mutable_feedback_id()); + output->set_status_flags(input.status_flags); + output->set_jitter_comm(input.jitter_comm); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_torque(input.torque); + output->set_current_motor(input.current_motor); + output->set_voltage(input.voltage); + output->set_temperature_motor(input.temperature_motor); + output->set_temperature_core(input.temperature_core); + output->set_fault_bank_a(input.fault_bank_a); + output->set_fault_bank_b(input.fault_bank_b); + output->set_warning_bank_a(input.warning_bank_a); + output->set_warning_bank_b(input.warning_bank_b); + + return 0; +} +int ToProtoData(kortex_actuator_driver::CustomData input, CustomData *output) +{ + ToProtoData(input.custom_data_id, output->mutable_custom_data_id()); + output->set_custom_data_0(input.custom_data_0); + output->set_custom_data_1(input.custom_data_1); + output->set_custom_data_2(input.custom_data_2); + output->set_custom_data_3(input.custom_data_3); + output->set_custom_data_4(input.custom_data_4); + output->set_custom_data_5(input.custom_data_5); + output->set_custom_data_6(input.custom_data_6); + output->set_custom_data_7(input.custom_data_7); + output->set_custom_data_8(input.custom_data_8); + output->set_custom_data_9(input.custom_data_9); + output->set_custom_data_10(input.custom_data_10); + output->set_custom_data_11(input.custom_data_11); + output->set_custom_data_12(input.custom_data_12); + output->set_custom_data_13(input.custom_data_13); + output->set_custom_data_14(input.custom_data_14); + output->set_custom_data_15(input.custom_data_15); + + return 0; +} diff --git a/kortex_actuator_driver/src/actuatorcyclic_proto_converter.h b/kortex_actuator_driver/src/actuatorcyclic_proto_converter.h new file mode 100644 index 00000000..ca58ab6f --- /dev/null +++ b/kortex_actuator_driver/src/actuatorcyclic_proto_converter.h @@ -0,0 +1,56 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ActuatorCyclicPROTO_CONVERTER_H_ +#define _KORTEX_ActuatorCyclicPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/MessageId.h" +#include "kortex_actuator_driver/Command.h" +#include "kortex_actuator_driver/Feedback.h" +#include "kortex_actuator_driver/CustomData.h" + + +using namespace Kinova::Api::ActuatorCyclic; + +int ToProtoData(kortex_actuator_driver::MessageId intput, MessageId *output); +int ToProtoData(kortex_actuator_driver::Command intput, Command *output); +int ToProtoData(kortex_actuator_driver::Feedback intput, Feedback *output); +int ToProtoData(kortex_actuator_driver::CustomData intput, CustomData *output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/actuatorcyclic_ros_converter.cpp b/kortex_actuator_driver/src/actuatorcyclic_ros_converter.cpp new file mode 100644 index 00000000..94228493 --- /dev/null +++ b/kortex_actuator_driver/src/actuatorcyclic_ros_converter.cpp @@ -0,0 +1,79 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "actuatorcyclic_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(MessageId input, kortex_actuator_driver::MessageId &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(Command input, kortex_actuator_driver::Command &output) +{ + ToRosData(input.command_id(), output.command_id); + output.flags = input.flags(); + output.position = input.position(); + output.velocity = input.velocity(); + output.torque_joint = input.torque_joint(); + output.current_motor = input.current_motor(); + + return 0; +} +int ToRosData(Feedback input, kortex_actuator_driver::Feedback &output) +{ + ToRosData(input.feedback_id(), output.feedback_id); + output.status_flags = input.status_flags(); + output.jitter_comm = input.jitter_comm(); + output.position = input.position(); + output.velocity = input.velocity(); + output.torque = input.torque(); + output.current_motor = input.current_motor(); + output.voltage = input.voltage(); + output.temperature_motor = input.temperature_motor(); + output.temperature_core = input.temperature_core(); + output.fault_bank_a = input.fault_bank_a(); + output.fault_bank_b = input.fault_bank_b(); + output.warning_bank_a = input.warning_bank_a(); + output.warning_bank_b = input.warning_bank_b(); + + return 0; +} +int ToRosData(CustomData input, kortex_actuator_driver::CustomData &output) +{ + ToRosData(input.custom_data_id(), output.custom_data_id); + output.custom_data_0 = input.custom_data_0(); + output.custom_data_1 = input.custom_data_1(); + output.custom_data_2 = input.custom_data_2(); + output.custom_data_3 = input.custom_data_3(); + output.custom_data_4 = input.custom_data_4(); + output.custom_data_5 = input.custom_data_5(); + output.custom_data_6 = input.custom_data_6(); + output.custom_data_7 = input.custom_data_7(); + output.custom_data_8 = input.custom_data_8(); + output.custom_data_9 = input.custom_data_9(); + output.custom_data_10 = input.custom_data_10(); + output.custom_data_11 = input.custom_data_11(); + output.custom_data_12 = input.custom_data_12(); + output.custom_data_13 = input.custom_data_13(); + output.custom_data_14 = input.custom_data_14(); + output.custom_data_15 = input.custom_data_15(); + + return 0; +} diff --git a/kortex_actuator_driver/src/actuatorcyclic_ros_converter.h b/kortex_actuator_driver/src/actuatorcyclic_ros_converter.h new file mode 100644 index 00000000..b96bb569 --- /dev/null +++ b/kortex_actuator_driver/src/actuatorcyclic_ros_converter.h @@ -0,0 +1,56 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_ActuatorCyclicROS_CONVERTER_H_ +#define _KORTEX_ActuatorCyclicROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/MessageId.h" +#include "kortex_actuator_driver/Command.h" +#include "kortex_actuator_driver/Feedback.h" +#include "kortex_actuator_driver/CustomData.h" + + +using namespace Kinova::Api::ActuatorCyclic; + +int ToRosData(MessageId input, kortex_actuator_driver::MessageId &output); +int ToRosData(Command input, kortex_actuator_driver::Command &output); +int ToRosData(Feedback input, kortex_actuator_driver::Feedback &output); +int ToRosData(CustomData input, kortex_actuator_driver::CustomData &output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/common_proto_converter.cpp b/kortex_actuator_driver/src/common_proto_converter.cpp new file mode 100644 index 00000000..51241a2c --- /dev/null +++ b/kortex_actuator_driver/src/common_proto_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_actuator_driver::DeviceHandle input, DeviceHandle *output) +{ + output->set_device_type((Kinova::Api::Common::DeviceTypes)input.device_type); + output->set_device_identifier(input.device_identifier); + output->set_order(input.order); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Empty input, Empty *output) +{ + + return 0; +} +int ToProtoData(kortex_actuator_driver::NotificationOptions input, NotificationOptions *output) +{ + output->set_type((Kinova::Api::Common::NotificationType)input.type); + output->set_rate_m_sec(input.rate_m_sec); + output->set_threshold_value(input.threshold_value); + + return 0; +} +int ToProtoData(kortex_actuator_driver::SafetyHandle input, SafetyHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_actuator_driver::NotificationHandle input, NotificationHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_actuator_driver::SafetyNotification input, SafetyNotification *output) +{ + ToProtoData(input.safety_handle, output->mutable_safety_handle()); + output->set_value((Kinova::Api::Common::SafetyStatusValue)input.value); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Timestamp input, Timestamp *output) +{ + output->set_sec(input.sec); + output->set_usec(input.usec); + + return 0; +} +int ToProtoData(kortex_actuator_driver::UserProfileHandle input, UserProfileHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_actuator_driver::Connection input, Connection *output) +{ + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_connection_information(input.connection_information); + output->set_connection_identifier(input.connection_identifier); + + return 0; +} diff --git a/kortex_actuator_driver/src/common_proto_converter.h b/kortex_actuator_driver/src/common_proto_converter.h new file mode 100644 index 00000000..4112a508 --- /dev/null +++ b/kortex_actuator_driver/src/common_proto_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonPROTO_CONVERTER_H_ +#define _KORTEX_CommonPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/DeviceHandle.h" +#include "kortex_actuator_driver/Empty.h" +#include "kortex_actuator_driver/NotificationOptions.h" +#include "kortex_actuator_driver/SafetyHandle.h" +#include "kortex_actuator_driver/NotificationHandle.h" +#include "kortex_actuator_driver/SafetyNotification.h" +#include "kortex_actuator_driver/Timestamp.h" +#include "kortex_actuator_driver/UserProfileHandle.h" +#include "kortex_actuator_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToProtoData(kortex_actuator_driver::DeviceHandle intput, DeviceHandle *output); +int ToProtoData(kortex_actuator_driver::Empty intput, Empty *output); +int ToProtoData(kortex_actuator_driver::NotificationOptions intput, NotificationOptions *output); +int ToProtoData(kortex_actuator_driver::SafetyHandle intput, SafetyHandle *output); +int ToProtoData(kortex_actuator_driver::NotificationHandle intput, NotificationHandle *output); +int ToProtoData(kortex_actuator_driver::SafetyNotification intput, SafetyNotification *output); +int ToProtoData(kortex_actuator_driver::Timestamp intput, Timestamp *output); +int ToProtoData(kortex_actuator_driver::UserProfileHandle intput, UserProfileHandle *output); +int ToProtoData(kortex_actuator_driver::Connection intput, Connection *output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/common_ros_converter.cpp b/kortex_actuator_driver/src/common_ros_converter.cpp new file mode 100644 index 00000000..57946201 --- /dev/null +++ b/kortex_actuator_driver/src/common_ros_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_ros_converter.h" + + +int ToRosData(DeviceHandle input, kortex_actuator_driver::DeviceHandle &output) +{ + output.device_type = input.device_type(); + output.device_identifier = input.device_identifier(); + output.order = input.order(); + + return 0; +} +int ToRosData(Empty input, kortex_actuator_driver::Empty &output) +{ + + return 0; +} +int ToRosData(NotificationOptions input, kortex_actuator_driver::NotificationOptions &output) +{ + output.type = input.type(); + output.rate_m_sec = input.rate_m_sec(); + output.threshold_value = input.threshold_value(); + + return 0; +} +int ToRosData(SafetyHandle input, kortex_actuator_driver::SafetyHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(NotificationHandle input, kortex_actuator_driver::NotificationHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(SafetyNotification input, kortex_actuator_driver::SafetyNotification &output) +{ + ToRosData(input.safety_handle(), output.safety_handle); + output.value = input.value(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(Timestamp input, kortex_actuator_driver::Timestamp &output) +{ + output.sec = input.sec(); + output.usec = input.usec(); + + return 0; +} +int ToRosData(UserProfileHandle input, kortex_actuator_driver::UserProfileHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(Connection input, kortex_actuator_driver::Connection &output) +{ + ToRosData(input.user_handle(), output.user_handle); + output.connection_information = input.connection_information(); + output.connection_identifier = input.connection_identifier(); + + return 0; +} diff --git a/kortex_actuator_driver/src/common_ros_converter.h b/kortex_actuator_driver/src/common_ros_converter.h new file mode 100644 index 00000000..505d5764 --- /dev/null +++ b/kortex_actuator_driver/src/common_ros_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonROS_CONVERTER_H_ +#define _KORTEX_CommonROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_actuator_driver/DeviceHandle.h" +#include "kortex_actuator_driver/Empty.h" +#include "kortex_actuator_driver/NotificationOptions.h" +#include "kortex_actuator_driver/SafetyHandle.h" +#include "kortex_actuator_driver/NotificationHandle.h" +#include "kortex_actuator_driver/SafetyNotification.h" +#include "kortex_actuator_driver/Timestamp.h" +#include "kortex_actuator_driver/UserProfileHandle.h" +#include "kortex_actuator_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToRosData(DeviceHandle input, kortex_actuator_driver::DeviceHandle &output); +int ToRosData(Empty input, kortex_actuator_driver::Empty &output); +int ToRosData(NotificationOptions input, kortex_actuator_driver::NotificationOptions &output); +int ToRosData(SafetyHandle input, kortex_actuator_driver::SafetyHandle &output); +int ToRosData(NotificationHandle input, kortex_actuator_driver::NotificationHandle &output); +int ToRosData(SafetyNotification input, kortex_actuator_driver::SafetyNotification &output); +int ToRosData(Timestamp input, kortex_actuator_driver::Timestamp &output); +int ToRosData(UserProfileHandle input, kortex_actuator_driver::UserProfileHandle &output); +int ToRosData(Connection input, kortex_actuator_driver::Connection &output); + +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/src/main.cpp b/kortex_actuator_driver/src/main.cpp new file mode 100644 index 00000000..ef974c06 --- /dev/null +++ b/kortex_actuator_driver/src/main.cpp @@ -0,0 +1,180 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Actuator_Services"); + + uint32_t cyclic_data_rate = 100; + uint32_t device_id = 0; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 3) + { + stringstream tempId; + tempId << argv[3]; + tempId >> device_id; + + if(tempId.fail() || tempId.bad()) + { + ROS_INFO("ERROR - Bad device ID, shutting down the node..."); + ros::shutdown(); + return 0; + } + } + else if(argc > 2) + { + //Converting the second parameter(the cyclic rate) to an unsigned int variable. + stringstream tempRate; + tempRate << argv[2]; + tempRate >> cyclic_data_rate; + if(tempRate.fail() || tempRate.bad()) + { + ROS_INFO("ERROR - Bad error rate, shutting down the node..."); + ros::shutdown(); + return 0; + } + ROS_INFO("Connecting to IP = %s - node refresh rate = %s, device ID = %d", argv[1], argv[2], device_id); + } + else + { + ROS_INFO("You need to provide an IP adresse as the first parameter and optionnaly a second parameter to specify the cyclic \ + rate and a third parameter(optionnal) to specify a device ID. ex: rosrun package node 127.0.0.1 100 4"); + ros::shutdown(); + return 0; + } + + Actuator_Services services_object(argv[1], n, device_id); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &Actuator_Services::SetDeviceID, &services_object); + ros::ServiceServer serviceSetApiOptions = n.advertiseService("SetApiOptions", &Actuator_Services::SetApiOptions, &services_object); + ros::ServiceServer serviceSetCyclicStatus = n.advertiseService("SetCyclicStatus", &Actuator_Services::SetCyclicStatus, &services_object); + ros::ServiceServer serviceGetCyclicStatus = n.advertiseService("GetCyclicStatus", &Actuator_Services::GetCyclicStatus, &services_object); + + ros::ServiceServer serviceGetAxisOffsets = n.advertiseService("GetAxisOffsets", &Actuator_Services::GetAxisOffsets, &services_object); + ros::ServiceServer serviceSetAxisOffsets = n.advertiseService("SetAxisOffsets", &Actuator_Services::SetAxisOffsets, &services_object); + ros::ServiceServer serviceReadTorqueCalibration = n.advertiseService("ReadTorqueCalibration", &Actuator_Services::ReadTorqueCalibration, &services_object); + ros::ServiceServer serviceWriteTorqueCalibration = n.advertiseService("WriteTorqueCalibration", &Actuator_Services::WriteTorqueCalibration, &services_object); + ros::ServiceServer serviceSetTorqueOffset = n.advertiseService("SetTorqueOffset", &Actuator_Services::SetTorqueOffset, &services_object); + ros::ServiceServer serviceGetControlMode = n.advertiseService("GetControlMode", &Actuator_Services::GetControlMode, &services_object); + ros::ServiceServer serviceSetControlMode = n.advertiseService("SetControlMode", &Actuator_Services::SetControlMode, &services_object); + ros::ServiceServer serviceGetActivatedControlLoop = n.advertiseService("GetActivatedControlLoop", &Actuator_Services::GetActivatedControlLoop, &services_object); + ros::ServiceServer serviceSetActivatedControlLoop = n.advertiseService("SetActivatedControlLoop", &Actuator_Services::SetActivatedControlLoop, &services_object); + ros::ServiceServer serviceGetVectorDriveParameters = n.advertiseService("GetVectorDriveParameters", &Actuator_Services::GetVectorDriveParameters, &services_object); + ros::ServiceServer serviceSetVectorDriveParameters = n.advertiseService("SetVectorDriveParameters", &Actuator_Services::SetVectorDriveParameters, &services_object); + ros::ServiceServer serviceGetEncoderDerivativeParameters = n.advertiseService("GetEncoderDerivativeParameters", &Actuator_Services::GetEncoderDerivativeParameters, &services_object); + ros::ServiceServer serviceSetEncoderDerivativeParameters = n.advertiseService("SetEncoderDerivativeParameters", &Actuator_Services::SetEncoderDerivativeParameters, &services_object); + ros::ServiceServer serviceGetControlLoopParameters = n.advertiseService("GetControlLoopParameters", &Actuator_Services::GetControlLoopParameters, &services_object); + ros::ServiceServer serviceSetControlLoopParameters = n.advertiseService("SetControlLoopParameters", &Actuator_Services::SetControlLoopParameters, &services_object); + ros::ServiceServer serviceStartFrequencyResponse = n.advertiseService("StartFrequencyResponse", &Actuator_Services::StartFrequencyResponse, &services_object); + ros::ServiceServer serviceStopFrequencyResponse = n.advertiseService("StopFrequencyResponse", &Actuator_Services::StopFrequencyResponse, &services_object); + ros::ServiceServer serviceStartStepResponse = n.advertiseService("StartStepResponse", &Actuator_Services::StartStepResponse, &services_object); + ros::ServiceServer serviceStopStepResponse = n.advertiseService("StopStepResponse", &Actuator_Services::StopStepResponse, &services_object); + ros::ServiceServer serviceStartRampResponse = n.advertiseService("StartRampResponse", &Actuator_Services::StartRampResponse, &services_object); + ros::ServiceServer serviceStopRampResponse = n.advertiseService("StopRampResponse", &Actuator_Services::StopRampResponse, &services_object); + ros::ServiceServer serviceSelectCustomData = n.advertiseService("SelectCustomData", &Actuator_Services::SelectCustomData, &services_object); + ros::ServiceServer serviceGetSelectedCustomData = n.advertiseService("GetSelectedCustomData", &Actuator_Services::GetSelectedCustomData, &services_object); + ros::ServiceServer serviceSetCommandMode = n.advertiseService("SetCommandMode", &Actuator_Services::SetCommandMode, &services_object); + ros::ServiceServer serviceClearFaults = n.advertiseService("ClearFaults", &Actuator_Services::ClearFaults, &services_object); + ros::ServiceServer serviceSetServoing = n.advertiseService("SetServoing", &Actuator_Services::SetServoing, &services_object); + ros::ServiceServer serviceMoveToPosition = n.advertiseService("MoveToPosition", &Actuator_Services::MoveToPosition, &services_object); + ros::ServiceServer serviceGetCommandMode = n.advertiseService("GetCommandMode", &Actuator_Services::GetCommandMode, &services_object); + ros::ServiceServer serviceGetServoing = n.advertiseService("GetServoing", &Actuator_Services::GetServoing, &services_object); + ros::ServiceServer serviceGetTorqueOffset = n.advertiseService("GetTorqueOffset", &Actuator_Services::GetTorqueOffset, &services_object); + ros::ServiceServer serviceRefresh = n.advertiseService("Refresh", &Actuator_Services::Refresh, &services_object); + ros::ServiceServer serviceRefreshCommand = n.advertiseService("RefreshCommand", &Actuator_Services::RefreshCommand, &services_object); + ros::ServiceServer serviceRefreshFeedback = n.advertiseService("RefreshFeedback", &Actuator_Services::RefreshFeedback, &services_object); + ros::ServiceServer serviceRefreshCustomData = n.advertiseService("RefreshCustomData", &Actuator_Services::RefreshCustomData, &services_object); + + + ROS_INFO("Node's services initialized correctly."); + + ros::Publisher pub_feedback = n.advertise("actuator_feedback", 1000); + ros::Publisher pub_joint_state = n.advertise("actuator_feedback/joint_state", 1000); + + kortex_actuator_driver::Feedback feedback; + kortex_actuator_driver::RefreshFeedback::Request req; + kortex_actuator_driver::RefreshFeedback::Response res; + + sensor_msgs::JointState joint_state; + + joint_state.position.resize(1); + joint_state.velocity.resize(1); + joint_state.effort.resize(1); + joint_state.name.resize(1); + + ros::Rate rate(cyclic_data_rate); // 100 hz + while (!ros::isShuttingDown()) + { + try + { + if(services_object.IsCyclicActive()) + { + services_object.RefreshFeedback(req, res); + + feedback.feedback_id = res.output.feedback_id; + + feedback.status_flags = res.output.status_flags; + feedback.jitter_comm = res.output.jitter_comm; + feedback.position = res.output.position; + feedback.velocity = res.output.velocity; + feedback.torque = res.output.torque; + feedback.current_motor = res.output.current_motor; + feedback.voltage = res.output.voltage; + feedback.temperature_motor = res.output.temperature_motor; + feedback.temperature_core = res.output.temperature_core; + feedback.fault_bank_a = res.output.fault_bank_a; + feedback.fault_bank_b = res.output.fault_bank_b; + feedback.warning_bank_a = res.output.warning_bank_a; + feedback.warning_bank_b = res.output.warning_bank_b; + + joint_state.header.stamp = ros::Time::now(); + joint_state.header.frame_id = std::to_string(res.output.feedback_id.identifier); + + joint_state.name[0] = "Actuator"; + joint_state.position[0] = TO_RAD(res.output.position); + joint_state.velocity[0] = TO_RAD(res.output.velocity); + joint_state.effort[0] = res.output.torque; + + pub_feedback.publish(feedback); + pub_joint_state.publish(joint_state); + } + } + + catch (KDetailedException& ex) + { + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + } + catch (std::runtime_error& ex2) + { + ROS_INFO("RUN TIME ERROR: %s\n", ex2.what()); + } + + ros::spinOnce(); + + rate.sleep(); + } + + return 1; +} \ No newline at end of file diff --git a/kortex_actuator_driver/src/node.cpp b/kortex_actuator_driver/src/node.cpp new file mode 100644 index 00000000..1b542590 --- /dev/null +++ b/kortex_actuator_driver/src/node.cpp @@ -0,0 +1,1009 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "common_ros_converter.h" +#include "common_proto_converter.h" +#include "actuatorconfig_ros_converter.h" +#include "actuatorconfig_proto_converter.h" +#include "actuatorcyclic_ros_converter.h" +#include "actuatorcyclic_proto_converter.h" +Actuator_Services::Actuator_Services(char* ip, ros::NodeHandle& n, uint32_t device_id) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = device_id; + m_apiOptions.timeout_ms = 3000; + + m_actuatorconfig = new ActuatorConfig::ActuatorConfigClient(m_router); + m_actuatorcyclic = new ActuatorCyclic::ActuatorCyclicClient(m_router);//If the Device ID is different than 0, it means that we are using the feature DEVICE ROUTING. + if(m_CurrentDeviceID != 0) + { + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + } + + m_pub_Error = m_n.advertise("KortexError", 1000);std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool Actuator_Services::SetDeviceID(kortex_actuator_driver::SetDeviceID::Request &req, kortex_actuator_driver::SetDeviceID::Response &res) +{ + if(m_CurrentDeviceID == 0) + { + auto sessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + sessionManager->CreateSession(createSessionInfo); + } + + m_CurrentDeviceID = req.device_id; +} + +bool Actuator_Services::SetApiOptions(kortex_actuator_driver::SetApiOptions::Request &req, kortex_actuator_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} + +bool Actuator_Services::GetCyclicStatus(kortex_actuator_driver::GetCyclicStatus::Request &req, kortex_actuator_driver::GetCyclicStatus::Response &res) +{ + res.status.isActive = m_cyclicActive; +} + +bool Actuator_Services::SetCyclicStatus(kortex_actuator_driver::SetCyclicStatus::Request &req, kortex_actuator_driver::SetCyclicStatus::Response &res) +{ + m_cyclicActive = req.status.isActive; +} + +bool Actuator_Services::IsCyclicActive() +{ + return m_cyclicActive; +} + + + + +bool Actuator_Services::GetAxisOffsets(kortex_actuator_driver::GetAxisOffsets::Request &req, kortex_actuator_driver::GetAxisOffsets::Response &res) +{ + Empty input; + AxisOffsets output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetAxisOffsets(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetAxisOffsets(kortex_actuator_driver::SetAxisOffsets::Request &req, kortex_actuator_driver::SetAxisOffsets::Response &res) +{ + AxisPosition input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetAxisOffsets(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::ReadTorqueCalibration(kortex_actuator_driver::ReadTorqueCalibration::Request &req, kortex_actuator_driver::ReadTorqueCalibration::Response &res) +{ + Empty input; + TorqueCalibration output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->ReadTorqueCalibration(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::WriteTorqueCalibration(kortex_actuator_driver::WriteTorqueCalibration::Request &req, kortex_actuator_driver::WriteTorqueCalibration::Response &res) +{ + TorqueCalibration input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->WriteTorqueCalibration(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::SetTorqueOffset(kortex_actuator_driver::SetTorqueOffset::Request &req, kortex_actuator_driver::SetTorqueOffset::Response &res) +{ + TorqueOffset input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetTorqueOffset(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetControlMode(kortex_actuator_driver::GetControlMode::Request &req, kortex_actuator_driver::GetControlMode::Response &res) +{ + Empty input; + ControlModeInformation output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetControlMode(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetControlMode(kortex_actuator_driver::SetControlMode::Request &req, kortex_actuator_driver::SetControlMode::Response &res) +{ + ControlModeInformation input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetControlMode(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetActivatedControlLoop(kortex_actuator_driver::GetActivatedControlLoop::Request &req, kortex_actuator_driver::GetActivatedControlLoop::Response &res) +{ + Empty input; + ControlLoop output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetActivatedControlLoop(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetActivatedControlLoop(kortex_actuator_driver::SetActivatedControlLoop::Request &req, kortex_actuator_driver::SetActivatedControlLoop::Response &res) +{ + ControlLoop input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetActivatedControlLoop(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetVectorDriveParameters(kortex_actuator_driver::GetVectorDriveParameters::Request &req, kortex_actuator_driver::GetVectorDriveParameters::Response &res) +{ + Empty input; + VectorDriveParameters output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetVectorDriveParameters(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetVectorDriveParameters(kortex_actuator_driver::SetVectorDriveParameters::Request &req, kortex_actuator_driver::SetVectorDriveParameters::Response &res) +{ + VectorDriveParameters input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetVectorDriveParameters(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetEncoderDerivativeParameters(kortex_actuator_driver::GetEncoderDerivativeParameters::Request &req, kortex_actuator_driver::GetEncoderDerivativeParameters::Response &res) +{ + Empty input; + EncoderDerivativeParameters output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetEncoderDerivativeParameters(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetEncoderDerivativeParameters(kortex_actuator_driver::SetEncoderDerivativeParameters::Request &req, kortex_actuator_driver::SetEncoderDerivativeParameters::Response &res) +{ + EncoderDerivativeParameters input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetEncoderDerivativeParameters(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetControlLoopParameters(kortex_actuator_driver::GetControlLoopParameters::Request &req, kortex_actuator_driver::GetControlLoopParameters::Response &res) +{ + LoopSelection input; + ToProtoData(req.input, &input); + ControlLoopParameters output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetControlLoopParameters(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetControlLoopParameters(kortex_actuator_driver::SetControlLoopParameters::Request &req, kortex_actuator_driver::SetControlLoopParameters::Response &res) +{ + ControlLoopParameters input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetControlLoopParameters(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StartFrequencyResponse(kortex_actuator_driver::StartFrequencyResponse::Request &req, kortex_actuator_driver::StartFrequencyResponse::Response &res) +{ + FrequencyResponse input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StartFrequencyResponse(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StopFrequencyResponse(kortex_actuator_driver::StopFrequencyResponse::Request &req, kortex_actuator_driver::StopFrequencyResponse::Response &res) +{ + Empty input; + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StopFrequencyResponse(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StartStepResponse(kortex_actuator_driver::StartStepResponse::Request &req, kortex_actuator_driver::StartStepResponse::Response &res) +{ + StepResponse input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StartStepResponse(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StopStepResponse(kortex_actuator_driver::StopStepResponse::Request &req, kortex_actuator_driver::StopStepResponse::Response &res) +{ + Empty input; + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StopStepResponse(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StartRampResponse(kortex_actuator_driver::StartRampResponse::Request &req, kortex_actuator_driver::StartRampResponse::Response &res) +{ + RampResponse input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StartRampResponse(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::StopRampResponse(kortex_actuator_driver::StopRampResponse::Request &req, kortex_actuator_driver::StopRampResponse::Response &res) +{ + Empty input; + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->StopRampResponse(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::SelectCustomData(kortex_actuator_driver::SelectCustomData::Request &req, kortex_actuator_driver::SelectCustomData::Response &res) +{ + CustomDataSelection input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SelectCustomData(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetSelectedCustomData(kortex_actuator_driver::GetSelectedCustomData::Request &req, kortex_actuator_driver::GetSelectedCustomData::Response &res) +{ + Empty input; + CustomDataSelection output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetSelectedCustomData(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::SetCommandMode(kortex_actuator_driver::SetCommandMode::Request &req, kortex_actuator_driver::SetCommandMode::Response &res) +{ + CommandModeInformation input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetCommandMode(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::ClearFaults(kortex_actuator_driver::ClearFaults::Request &req, kortex_actuator_driver::ClearFaults::Response &res) +{ + Empty input; + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->ClearFaults(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::SetServoing(kortex_actuator_driver::SetServoing::Request &req, kortex_actuator_driver::SetServoing::Response &res) +{ + Servoing input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->SetServoing(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::MoveToPosition(kortex_actuator_driver::MoveToPosition::Request &req, kortex_actuator_driver::MoveToPosition::Response &res) +{ + PositionCommand input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorconfig->MoveToPosition(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::GetCommandMode(kortex_actuator_driver::GetCommandMode::Request &req, kortex_actuator_driver::GetCommandMode::Response &res) +{ + Empty input; + CommandModeInformation output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetCommandMode(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::GetServoing(kortex_actuator_driver::GetServoing::Request &req, kortex_actuator_driver::GetServoing::Response &res) +{ + Empty input; + Servoing output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetServoing(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::GetTorqueOffset(kortex_actuator_driver::GetTorqueOffset::Request &req, kortex_actuator_driver::GetTorqueOffset::Response &res) +{ + Empty input; + TorqueOffset output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorconfig->GetTorqueOffset(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + + +bool Actuator_Services::Refresh(kortex_actuator_driver::Refresh::Request &req, kortex_actuator_driver::Refresh::Response &res) +{ + Command input; + ToProtoData(req.input, &input); + Feedback output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorcyclic->Refresh(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::RefreshCommand(kortex_actuator_driver::RefreshCommand::Request &req, kortex_actuator_driver::RefreshCommand::Response &res) +{ + Command input; + ToProtoData(req.input, &input); + Empty output; + kortex_actuator_driver::KortexError result_error; + + try + { + m_actuatorcyclic->RefreshCommand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool Actuator_Services::RefreshFeedback(kortex_actuator_driver::RefreshFeedback::Request &req, kortex_actuator_driver::RefreshFeedback::Response &res) +{ + MessageId input; + ToProtoData(req.input, &input); + Feedback output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorcyclic->RefreshFeedback(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool Actuator_Services::RefreshCustomData(kortex_actuator_driver::RefreshCustomData::Request &req, kortex_actuator_driver::RefreshCustomData::Response &res) +{ + MessageId input; + ToProtoData(req.input, &input); + CustomData output; + kortex_actuator_driver::KortexError result_error; + + try + { + output = m_actuatorcyclic->RefreshCustomData(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_actuator_driver/src/node.h b/kortex_actuator_driver/src/node.h new file mode 100644 index 00000000..6c743e23 --- /dev/null +++ b/kortex_actuator_driver/src/node.h @@ -0,0 +1,154 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_SERVICES_H_ +#define _KORTEX_SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include "kortex_actuator_driver/GetAxisOffsets.h" +#include "kortex_actuator_driver/SetAxisOffsets.h" +#include "kortex_actuator_driver/ReadTorqueCalibration.h" +#include "kortex_actuator_driver/WriteTorqueCalibration.h" +#include "kortex_actuator_driver/SetTorqueOffset.h" +#include "kortex_actuator_driver/GetControlMode.h" +#include "kortex_actuator_driver/SetControlMode.h" +#include "kortex_actuator_driver/GetActivatedControlLoop.h" +#include "kortex_actuator_driver/SetActivatedControlLoop.h" +#include "kortex_actuator_driver/GetVectorDriveParameters.h" +#include "kortex_actuator_driver/SetVectorDriveParameters.h" +#include "kortex_actuator_driver/GetEncoderDerivativeParameters.h" +#include "kortex_actuator_driver/SetEncoderDerivativeParameters.h" +#include "kortex_actuator_driver/GetControlLoopParameters.h" +#include "kortex_actuator_driver/SetControlLoopParameters.h" +#include "kortex_actuator_driver/StartFrequencyResponse.h" +#include "kortex_actuator_driver/StopFrequencyResponse.h" +#include "kortex_actuator_driver/StartStepResponse.h" +#include "kortex_actuator_driver/StopStepResponse.h" +#include "kortex_actuator_driver/StartRampResponse.h" +#include "kortex_actuator_driver/StopRampResponse.h" +#include "kortex_actuator_driver/SelectCustomData.h" +#include "kortex_actuator_driver/GetSelectedCustomData.h" +#include "kortex_actuator_driver/SetCommandMode.h" +#include "kortex_actuator_driver/ClearFaults.h" +#include "kortex_actuator_driver/SetServoing.h" +#include "kortex_actuator_driver/MoveToPosition.h" +#include "kortex_actuator_driver/GetCommandMode.h" +#include "kortex_actuator_driver/GetServoing.h" +#include "kortex_actuator_driver/GetTorqueOffset.h" +#include "kortex_actuator_driver/Refresh.h" +#include "kortex_actuator_driver/RefreshCommand.h" +#include "kortex_actuator_driver/RefreshFeedback.h" +#include "kortex_actuator_driver/RefreshCustomData.h" +#include "kortex_actuator_driver/KortexError.h" +#include "kortex_actuator_driver/SetDeviceID.h" +#include "kortex_actuator_driver/SetApiOptions.h" +#include "kortex_actuator_driver/SetCyclicStatus.h" +#include "kortex_actuator_driver/GetCyclicStatus.h" +#include "kortex_actuator_driver/ApiOptions.h" +#include "kortex_actuator_driver/CyclicStatus.h" + +using namespace std; +using namespace Kinova::Api; +using namespace Kinova::Api::Common; +using namespace Kinova::Api::ActuatorConfig; +using namespace Kinova::Api::ActuatorCyclic; + +class Actuator_Services +{ + public: + Actuator_Services(char* ip, ros::NodeHandle& n, uint32_t device_id); + bool SetDeviceID(kortex_actuator_driver::SetDeviceID::Request &req, kortex_actuator_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_actuator_driver::SetApiOptions::Request &req, kortex_actuator_driver::SetApiOptions::Response &res); + bool GetCyclicStatus(kortex_actuator_driver::GetCyclicStatus::Request &req, kortex_actuator_driver::GetCyclicStatus::Response &res); + bool SetCyclicStatus(kortex_actuator_driver::SetCyclicStatus::Request &req, kortex_actuator_driver::SetCyclicStatus::Response &res); + bool IsCyclicActive(); + + + bool GetAxisOffsets(kortex_actuator_driver::GetAxisOffsets::Request &req, kortex_actuator_driver::GetAxisOffsets::Response &res); + bool SetAxisOffsets(kortex_actuator_driver::SetAxisOffsets::Request &req, kortex_actuator_driver::SetAxisOffsets::Response &res); + bool ReadTorqueCalibration(kortex_actuator_driver::ReadTorqueCalibration::Request &req, kortex_actuator_driver::ReadTorqueCalibration::Response &res); + bool WriteTorqueCalibration(kortex_actuator_driver::WriteTorqueCalibration::Request &req, kortex_actuator_driver::WriteTorqueCalibration::Response &res); + bool SetTorqueOffset(kortex_actuator_driver::SetTorqueOffset::Request &req, kortex_actuator_driver::SetTorqueOffset::Response &res); + bool GetControlMode(kortex_actuator_driver::GetControlMode::Request &req, kortex_actuator_driver::GetControlMode::Response &res); + bool SetControlMode(kortex_actuator_driver::SetControlMode::Request &req, kortex_actuator_driver::SetControlMode::Response &res); + bool GetActivatedControlLoop(kortex_actuator_driver::GetActivatedControlLoop::Request &req, kortex_actuator_driver::GetActivatedControlLoop::Response &res); + bool SetActivatedControlLoop(kortex_actuator_driver::SetActivatedControlLoop::Request &req, kortex_actuator_driver::SetActivatedControlLoop::Response &res); + bool GetVectorDriveParameters(kortex_actuator_driver::GetVectorDriveParameters::Request &req, kortex_actuator_driver::GetVectorDriveParameters::Response &res); + bool SetVectorDriveParameters(kortex_actuator_driver::SetVectorDriveParameters::Request &req, kortex_actuator_driver::SetVectorDriveParameters::Response &res); + bool GetEncoderDerivativeParameters(kortex_actuator_driver::GetEncoderDerivativeParameters::Request &req, kortex_actuator_driver::GetEncoderDerivativeParameters::Response &res); + bool SetEncoderDerivativeParameters(kortex_actuator_driver::SetEncoderDerivativeParameters::Request &req, kortex_actuator_driver::SetEncoderDerivativeParameters::Response &res); + bool GetControlLoopParameters(kortex_actuator_driver::GetControlLoopParameters::Request &req, kortex_actuator_driver::GetControlLoopParameters::Response &res); + bool SetControlLoopParameters(kortex_actuator_driver::SetControlLoopParameters::Request &req, kortex_actuator_driver::SetControlLoopParameters::Response &res); + bool StartFrequencyResponse(kortex_actuator_driver::StartFrequencyResponse::Request &req, kortex_actuator_driver::StartFrequencyResponse::Response &res); + bool StopFrequencyResponse(kortex_actuator_driver::StopFrequencyResponse::Request &req, kortex_actuator_driver::StopFrequencyResponse::Response &res); + bool StartStepResponse(kortex_actuator_driver::StartStepResponse::Request &req, kortex_actuator_driver::StartStepResponse::Response &res); + bool StopStepResponse(kortex_actuator_driver::StopStepResponse::Request &req, kortex_actuator_driver::StopStepResponse::Response &res); + bool StartRampResponse(kortex_actuator_driver::StartRampResponse::Request &req, kortex_actuator_driver::StartRampResponse::Response &res); + bool StopRampResponse(kortex_actuator_driver::StopRampResponse::Request &req, kortex_actuator_driver::StopRampResponse::Response &res); + bool SelectCustomData(kortex_actuator_driver::SelectCustomData::Request &req, kortex_actuator_driver::SelectCustomData::Response &res); + bool GetSelectedCustomData(kortex_actuator_driver::GetSelectedCustomData::Request &req, kortex_actuator_driver::GetSelectedCustomData::Response &res); + bool SetCommandMode(kortex_actuator_driver::SetCommandMode::Request &req, kortex_actuator_driver::SetCommandMode::Response &res); + bool ClearFaults(kortex_actuator_driver::ClearFaults::Request &req, kortex_actuator_driver::ClearFaults::Response &res); + bool SetServoing(kortex_actuator_driver::SetServoing::Request &req, kortex_actuator_driver::SetServoing::Response &res); + bool MoveToPosition(kortex_actuator_driver::MoveToPosition::Request &req, kortex_actuator_driver::MoveToPosition::Response &res); + bool GetCommandMode(kortex_actuator_driver::GetCommandMode::Request &req, kortex_actuator_driver::GetCommandMode::Response &res); + bool GetServoing(kortex_actuator_driver::GetServoing::Request &req, kortex_actuator_driver::GetServoing::Response &res); + bool GetTorqueOffset(kortex_actuator_driver::GetTorqueOffset::Request &req, kortex_actuator_driver::GetTorqueOffset::Response &res); + + bool Refresh(kortex_actuator_driver::Refresh::Request &req, kortex_actuator_driver::Refresh::Response &res); + bool RefreshCommand(kortex_actuator_driver::RefreshCommand::Request &req, kortex_actuator_driver::RefreshCommand::Response &res); + bool RefreshFeedback(kortex_actuator_driver::RefreshFeedback::Request &req, kortex_actuator_driver::RefreshFeedback::Response &res); + bool RefreshCustomData(kortex_actuator_driver::RefreshCustomData::Request &req, kortex_actuator_driver::RefreshCustomData::Response &res); + + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + + ActuatorConfigClient* m_actuatorconfig; + ActuatorCyclicClient* m_actuatorcyclic; + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + bool m_cyclicActive = false; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; +}; +#endif diff --git a/kortex_actuator_driver/src/util/diagnostic.h b/kortex_actuator_driver/src/util/diagnostic.h new file mode 100644 index 00000000..f0199870 --- /dev/null +++ b/kortex_actuator_driver/src/util/diagnostic.h @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +struct kortex_error +{ + int error_code; + std::string description; +}; \ No newline at end of file diff --git a/kortex_actuator_driver/src/util/math_util.h b/kortex_actuator_driver/src/util/math_util.h new file mode 100644 index 00000000..bf935386 --- /dev/null +++ b/kortex_actuator_driver/src/util/math_util.h @@ -0,0 +1,12 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +#define TO_RAD(degree) degree * M_PI / 180.0 \ No newline at end of file diff --git a/kortex_actuator_driver/srv/ClearFaults.srv b/kortex_actuator_driver/srv/ClearFaults.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_actuator_driver/srv/ClearFaults.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetActivatedControlLoop.srv b/kortex_actuator_driver/srv/GetActivatedControlLoop.srv new file mode 100644 index 00000000..a19e2136 --- /dev/null +++ b/kortex_actuator_driver/srv/GetActivatedControlLoop.srv @@ -0,0 +1,3 @@ +Empty input +--- +ControlLoop output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetAxisOffsets.srv b/kortex_actuator_driver/srv/GetAxisOffsets.srv new file mode 100644 index 00000000..4735b95e --- /dev/null +++ b/kortex_actuator_driver/srv/GetAxisOffsets.srv @@ -0,0 +1,3 @@ +Empty input +--- +AxisOffsets output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetCommandMode.srv b/kortex_actuator_driver/srv/GetCommandMode.srv new file mode 100644 index 00000000..bdfef0a5 --- /dev/null +++ b/kortex_actuator_driver/srv/GetCommandMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +CommandModeInformation output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetControlLoopParameters.srv b/kortex_actuator_driver/srv/GetControlLoopParameters.srv new file mode 100644 index 00000000..d999a54e --- /dev/null +++ b/kortex_actuator_driver/srv/GetControlLoopParameters.srv @@ -0,0 +1,3 @@ +LoopSelection input +--- +ControlLoopParameters output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetControlMode.srv b/kortex_actuator_driver/srv/GetControlMode.srv new file mode 100644 index 00000000..6eb15fb1 --- /dev/null +++ b/kortex_actuator_driver/srv/GetControlMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +ControlModeInformation output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetEncoderDerivativeParameters.srv b/kortex_actuator_driver/srv/GetEncoderDerivativeParameters.srv new file mode 100644 index 00000000..7815939b --- /dev/null +++ b/kortex_actuator_driver/srv/GetEncoderDerivativeParameters.srv @@ -0,0 +1,3 @@ +Empty input +--- +EncoderDerivativeParameters output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetSelectedCustomData.srv b/kortex_actuator_driver/srv/GetSelectedCustomData.srv new file mode 100644 index 00000000..cdc73093 --- /dev/null +++ b/kortex_actuator_driver/srv/GetSelectedCustomData.srv @@ -0,0 +1,3 @@ +Empty input +--- +CustomDataSelection output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetServoing.srv b/kortex_actuator_driver/srv/GetServoing.srv new file mode 100644 index 00000000..d4a6734b --- /dev/null +++ b/kortex_actuator_driver/srv/GetServoing.srv @@ -0,0 +1,3 @@ +Empty input +--- +Servoing output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetTorqueOffset.srv b/kortex_actuator_driver/srv/GetTorqueOffset.srv new file mode 100644 index 00000000..f8d55a75 --- /dev/null +++ b/kortex_actuator_driver/srv/GetTorqueOffset.srv @@ -0,0 +1,3 @@ +Empty input +--- +TorqueOffset output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/GetVectorDriveParameters.srv b/kortex_actuator_driver/srv/GetVectorDriveParameters.srv new file mode 100644 index 00000000..e6ea88c5 --- /dev/null +++ b/kortex_actuator_driver/srv/GetVectorDriveParameters.srv @@ -0,0 +1,3 @@ +Empty input +--- +VectorDriveParameters output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/MoveToPosition.srv b/kortex_actuator_driver/srv/MoveToPosition.srv new file mode 100644 index 00000000..bca986c7 --- /dev/null +++ b/kortex_actuator_driver/srv/MoveToPosition.srv @@ -0,0 +1,3 @@ +PositionCommand input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/ReadTorqueCalibration.srv b/kortex_actuator_driver/srv/ReadTorqueCalibration.srv new file mode 100644 index 00000000..45dec9f4 --- /dev/null +++ b/kortex_actuator_driver/srv/ReadTorqueCalibration.srv @@ -0,0 +1,3 @@ +Empty input +--- +TorqueCalibration output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/Refresh.srv b/kortex_actuator_driver/srv/Refresh.srv new file mode 100644 index 00000000..432de516 --- /dev/null +++ b/kortex_actuator_driver/srv/Refresh.srv @@ -0,0 +1,3 @@ +Command input +--- +Feedback output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/RefreshCommand.srv b/kortex_actuator_driver/srv/RefreshCommand.srv new file mode 100644 index 00000000..9bd2e9fc --- /dev/null +++ b/kortex_actuator_driver/srv/RefreshCommand.srv @@ -0,0 +1,3 @@ +Command input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/RefreshCustomData.srv b/kortex_actuator_driver/srv/RefreshCustomData.srv new file mode 100644 index 00000000..5822ae31 --- /dev/null +++ b/kortex_actuator_driver/srv/RefreshCustomData.srv @@ -0,0 +1,3 @@ +MessageId input +--- +CustomData output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/RefreshFeedback.srv b/kortex_actuator_driver/srv/RefreshFeedback.srv new file mode 100644 index 00000000..f850c63d --- /dev/null +++ b/kortex_actuator_driver/srv/RefreshFeedback.srv @@ -0,0 +1,3 @@ +MessageId input +--- +Feedback output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SelectCustomData.srv b/kortex_actuator_driver/srv/SelectCustomData.srv new file mode 100644 index 00000000..0bf1dfab --- /dev/null +++ b/kortex_actuator_driver/srv/SelectCustomData.srv @@ -0,0 +1,3 @@ +CustomDataSelection input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetActivatedControlLoop.srv b/kortex_actuator_driver/srv/SetActivatedControlLoop.srv new file mode 100644 index 00000000..14dbadcd --- /dev/null +++ b/kortex_actuator_driver/srv/SetActivatedControlLoop.srv @@ -0,0 +1,3 @@ +ControlLoop input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetAxisOffsets.srv b/kortex_actuator_driver/srv/SetAxisOffsets.srv new file mode 100644 index 00000000..00602436 --- /dev/null +++ b/kortex_actuator_driver/srv/SetAxisOffsets.srv @@ -0,0 +1,3 @@ +AxisPosition input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetCommandMode.srv b/kortex_actuator_driver/srv/SetCommandMode.srv new file mode 100644 index 00000000..9ba28a53 --- /dev/null +++ b/kortex_actuator_driver/srv/SetCommandMode.srv @@ -0,0 +1,3 @@ +CommandModeInformation input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetControlLoopParameters.srv b/kortex_actuator_driver/srv/SetControlLoopParameters.srv new file mode 100644 index 00000000..8f7419af --- /dev/null +++ b/kortex_actuator_driver/srv/SetControlLoopParameters.srv @@ -0,0 +1,3 @@ +ControlLoopParameters input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetControlMode.srv b/kortex_actuator_driver/srv/SetControlMode.srv new file mode 100644 index 00000000..d5a258a6 --- /dev/null +++ b/kortex_actuator_driver/srv/SetControlMode.srv @@ -0,0 +1,3 @@ +ControlModeInformation input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetEncoderDerivativeParameters.srv b/kortex_actuator_driver/srv/SetEncoderDerivativeParameters.srv new file mode 100644 index 00000000..d6d6530d --- /dev/null +++ b/kortex_actuator_driver/srv/SetEncoderDerivativeParameters.srv @@ -0,0 +1,3 @@ +EncoderDerivativeParameters input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetServoing.srv b/kortex_actuator_driver/srv/SetServoing.srv new file mode 100644 index 00000000..3eb88018 --- /dev/null +++ b/kortex_actuator_driver/srv/SetServoing.srv @@ -0,0 +1,3 @@ +Servoing input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetTorqueOffset.srv b/kortex_actuator_driver/srv/SetTorqueOffset.srv new file mode 100644 index 00000000..6f2857b9 --- /dev/null +++ b/kortex_actuator_driver/srv/SetTorqueOffset.srv @@ -0,0 +1,3 @@ +TorqueOffset input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/SetVectorDriveParameters.srv b/kortex_actuator_driver/srv/SetVectorDriveParameters.srv new file mode 100644 index 00000000..68aba166 --- /dev/null +++ b/kortex_actuator_driver/srv/SetVectorDriveParameters.srv @@ -0,0 +1,3 @@ +VectorDriveParameters input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StartFrequencyResponse.srv b/kortex_actuator_driver/srv/StartFrequencyResponse.srv new file mode 100644 index 00000000..38761145 --- /dev/null +++ b/kortex_actuator_driver/srv/StartFrequencyResponse.srv @@ -0,0 +1,3 @@ +FrequencyResponse input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StartRampResponse.srv b/kortex_actuator_driver/srv/StartRampResponse.srv new file mode 100644 index 00000000..246d7fa3 --- /dev/null +++ b/kortex_actuator_driver/srv/StartRampResponse.srv @@ -0,0 +1,3 @@ +RampResponse input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StartStepResponse.srv b/kortex_actuator_driver/srv/StartStepResponse.srv new file mode 100644 index 00000000..d5f063e6 --- /dev/null +++ b/kortex_actuator_driver/srv/StartStepResponse.srv @@ -0,0 +1,3 @@ +StepResponse input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StopFrequencyResponse.srv b/kortex_actuator_driver/srv/StopFrequencyResponse.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_actuator_driver/srv/StopFrequencyResponse.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StopRampResponse.srv b/kortex_actuator_driver/srv/StopRampResponse.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_actuator_driver/srv/StopRampResponse.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/StopStepResponse.srv b/kortex_actuator_driver/srv/StopStepResponse.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_actuator_driver/srv/StopStepResponse.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/WriteTorqueCalibration.srv b/kortex_actuator_driver/srv/WriteTorqueCalibration.srv new file mode 100644 index 00000000..cf4dac5d --- /dev/null +++ b/kortex_actuator_driver/srv/WriteTorqueCalibration.srv @@ -0,0 +1,3 @@ +TorqueCalibration input +--- +Empty output \ No newline at end of file diff --git a/kortex_actuator_driver/srv/non_generated/GetCyclicStatus.srv b/kortex_actuator_driver/srv/non_generated/GetCyclicStatus.srv new file mode 100644 index 00000000..2ec06d8c --- /dev/null +++ b/kortex_actuator_driver/srv/non_generated/GetCyclicStatus.srv @@ -0,0 +1,2 @@ +--- +CyclicStatus status \ No newline at end of file diff --git a/kortex_actuator_driver/srv/non_generated/SetApiOptions.srv b/kortex_actuator_driver/srv/non_generated/SetApiOptions.srv new file mode 100644 index 00000000..cab7c810 --- /dev/null +++ b/kortex_actuator_driver/srv/non_generated/SetApiOptions.srv @@ -0,0 +1,3 @@ +ApiOptions input +--- + diff --git a/kortex_actuator_driver/srv/non_generated/SetCyclicStatus.srv b/kortex_actuator_driver/srv/non_generated/SetCyclicStatus.srv new file mode 100644 index 00000000..05417e2c --- /dev/null +++ b/kortex_actuator_driver/srv/non_generated/SetCyclicStatus.srv @@ -0,0 +1,2 @@ +CyclicStatus status +--- \ No newline at end of file diff --git a/kortex_actuator_driver/srv/non_generated/SetDeviceID.srv b/kortex_actuator_driver/srv/non_generated/SetDeviceID.srv new file mode 100644 index 00000000..396957c5 --- /dev/null +++ b/kortex_actuator_driver/srv/non_generated/SetDeviceID.srv @@ -0,0 +1,2 @@ +uint32 device_id +--- diff --git a/kortex_actuator_driver/templates/NodeServices.cpp.jinja2 b/kortex_actuator_driver/templates/NodeServices.cpp.jinja2 new file mode 100644 index 00000000..9105fde0 --- /dev/null +++ b/kortex_actuator_driver/templates/NodeServices.cpp.jinja2 @@ -0,0 +1,167 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +{% for package in detailedPackages %} +{%- if package.HasMessage == 1 -%} +#include "{{package.filename|lower}}_ros_converter.h" +#include "{{package.filename|lower}}_proto_converter.h" +{%- endif %} +{% endfor -%} + +Actuator_Services::Actuator_Services(char* ip, ros::NodeHandle& n, uint32_t device_id) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = device_id; + m_apiOptions.timeout_ms = 3000; +{% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + m_{{package.name|lower}} = new {{package.name}}::{{package.name}}Client(m_router); + {%- endif -%} +{% endfor -%} + //If the Device ID is different than 0, it means that we are using the feature DEVICE ROUTING. + if(m_CurrentDeviceID != 0) + { + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + } + + m_pub_Error = m_n.advertise("KortexError", 1000); +{%- for package in detailedPackages -%} +{%- for method in package.service.method -%} +{%- if 'Topic' in method.name %} + m_pub_{{method.name}} = m_n.advertise("{{method.name}}", 1000); +{%- endif -%} +{%- endfor -%} +{%- endfor -%} + + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool Actuator_Services::SetDeviceID(kortex_actuator_driver::SetDeviceID::Request &req, kortex_actuator_driver::SetDeviceID::Response &res) +{ + if(m_CurrentDeviceID == 0) + { + auto sessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + sessionManager->CreateSession(createSessionInfo); + } + + m_CurrentDeviceID = req.device_id; +} + +bool Actuator_Services::SetApiOptions(kortex_actuator_driver::SetApiOptions::Request &req, kortex_actuator_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} + +bool Actuator_Services::GetCyclicStatus(kortex_actuator_driver::GetCyclicStatus::Request &req, kortex_actuator_driver::GetCyclicStatus::Response &res) +{ + res.status.isActive = m_cyclicActive; +} + +bool Actuator_Services::SetCyclicStatus(kortex_actuator_driver::SetCyclicStatus::Request &req, kortex_actuator_driver::SetCyclicStatus::Response &res) +{ + m_cyclicActive = req.status.isActive; +} + +bool Actuator_Services::IsCyclicActive() +{ + return m_cyclicActive; +} + +{% for package in detailedPackages %} +{% for method in package.service.method %} +{%- if 'Topic' in method.name %} +bool Actuator_Services::OnNotification{{method.name}}(kortex_actuator_driver::{{method.name}}::Request &req, kortex_actuator_driver::{{method.name}}::Response &res) +{%- else %} +bool Actuator_Services::{{method.name}}(kortex_actuator_driver::{{method.name}}::Request &req, kortex_actuator_driver::{{method.name}}::Response &res) +{%- endif %} +{ + {%- set splitInputTypeName = method.input_type.split('.') -%} + {% set splitOutputTypeName = method.output_type.split('.') %} + {{splitInputTypeName[4]}} input; + {%- if not method.input_type.split('.')[4] == "Empty" %} + ToProtoData(req.input, &input); + {%- endif %} + {{splitOutputTypeName[4]}} output; + kortex_actuator_driver::KortexError result_error; + + try + { + {%- if not method.output_type.split('.')[4] == "Empty" %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + {%- if 'Topic' in method.name %} + std::function< void ({{package.name}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&BaseServices::cb_{{method.name}}, this, std::placeholders::_1); + output = m_{{package.name|lower}}->OnNotification{{method.name}}(callback, input); + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- else %} + m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- endif %} + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + {%- if not method.output_type.split('.')[4] == "Empty" %} + ToRosData(output, res.output); + {%- endif %} + return true; +} +{%- if 'Topic' in method.name %} +void Actuator_Services::cb_{{method.name}}({{package.name}}::{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_actuator_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} +{% endfor -%} \ No newline at end of file diff --git a/kortex_actuator_driver/templates/NodeServices.h.jinja2 b/kortex_actuator_driver/templates/NodeServices.h.jinja2 new file mode 100644 index 00000000..1b7ca737 --- /dev/null +++ b/kortex_actuator_driver/templates/NodeServices.h.jinja2 @@ -0,0 +1,113 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{packageName}}SERVICES_H_ +#define _KORTEX_{{packageName}}SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +{%- for package in detailedPackages %} +#include <{{package.filename}}.pb.h> +{%- endfor %} + +#include +#include + +#include +#include + +{%- for package in detailedPackages %} +{%- if package.HasRPC == 1 %} +#include <{{package.name}}ClientRpc.h> +{%- endif %} +{%- endfor %} +#include +#include + +{%- for package in detailedPackages %} +{%- for method in package.service.method %} +#include "kortex_actuator_driver/{{method.name}}.h" +{%- endfor %} +{%- endfor %} +#include "kortex_actuator_driver/KortexError.h" +#include "kortex_actuator_driver/SetDeviceID.h" +#include "kortex_actuator_driver/SetApiOptions.h" +#include "kortex_actuator_driver/SetCyclicStatus.h" +#include "kortex_actuator_driver/GetCyclicStatus.h" +#include "kortex_actuator_driver/ApiOptions.h" +#include "kortex_actuator_driver/CyclicStatus.h" + +using namespace std; +using namespace Kinova::Api; +{%- for package in detailedPackages %} +using namespace {{package.namespace}}; +{%- endfor %} + +class Actuator_Services +{ + public: + Actuator_Services(char* ip, ros::NodeHandle& n, uint32_t device_id); + bool SetDeviceID(kortex_actuator_driver::SetDeviceID::Request &req, kortex_actuator_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_actuator_driver::SetApiOptions::Request &req, kortex_actuator_driver::SetApiOptions::Response &res); + bool GetCyclicStatus(kortex_actuator_driver::GetCyclicStatus::Request &req, kortex_actuator_driver::GetCyclicStatus::Response &res); + bool SetCyclicStatus(kortex_actuator_driver::SetCyclicStatus::Request &req, kortex_actuator_driver::SetCyclicStatus::Response &res); + bool IsCyclicActive(); +{% for package in detailedPackages %} +{%- for method in package.service.method %} +{%- if 'Topic' in method.name %} + bool OnNotification{{method.name}}(kortex_actuator_driver::{{method.name}}::Request &req, kortex_actuator_driver::{{method.name}}::Response &res); + void cb_{{method.name}}({{method.name|replace("Topic", "")}}Notification notif); +{%- else %} + bool {{method.name}}(kortex_actuator_driver::{{method.name}}::Request &req, kortex_actuator_driver::{{method.name}}::Response &res); +{%- endif %} +{%- endfor %} +{% endfor %} + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + {% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + {{package.name}}Client* m_{{package.name|lower}}; + {%- endif -%} + {% endfor %} + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + bool m_cyclicActive = false; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + + {%- for package in detailedPackages %} + {%- for method in package.service.method %} + {%- if 'Topic' in method.name %} + ros::Publisher m_pub_{{method.name}}; + {%- endif %} + {%- endfor %} + {%- endfor %} +}; +#endif + diff --git a/kortex_actuator_driver/templates/main.jinja2 b/kortex_actuator_driver/templates/main.jinja2 new file mode 100644 index 00000000..3accd393 --- /dev/null +++ b/kortex_actuator_driver/templates/main.jinja2 @@ -0,0 +1,148 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Actuator_Services"); + + uint32_t cyclic_data_rate = 100; + uint32_t device_id = 0; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 3) + { + stringstream tempId; + tempId << argv[3]; + tempId >> device_id; + + if(tempId.fail() || tempId.bad()) + { + ROS_INFO("ERROR - Bad device ID, shutting down the node..."); + ros::shutdown(); + return 0; + } + } + else if(argc > 2) + { + //Converting the second parameter(the cyclic rate) to an unsigned int variable. + stringstream tempRate; + tempRate << argv[2]; + tempRate >> cyclic_data_rate; + if(tempRate.fail() || tempRate.bad()) + { + ROS_INFO("ERROR - Bad error rate, shutting down the node..."); + ros::shutdown(); + return 0; + } + ROS_INFO("Connecting to IP = %s - node refresh rate = %s, device ID = %d", argv[1], argv[2], device_id); + } + else + { + ROS_INFO("You need to provide an IP adresse as the first parameter and optionnaly a second parameter to specify the cyclic \ + rate and a third parameter(optionnal) to specify a device ID. ex: rosrun package node 127.0.0.1 100 4"); + ros::shutdown(); + return 0; + } + + Actuator_Services services_object(argv[1], n, device_id); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &Actuator_Services::SetDeviceID, &services_object); + ros::ServiceServer serviceSetApiOptions = n.advertiseService("SetApiOptions", &Actuator_Services::SetApiOptions, &services_object); + ros::ServiceServer serviceSetCyclicStatus = n.advertiseService("SetCyclicStatus", &Actuator_Services::SetCyclicStatus, &services_object); + ros::ServiceServer serviceGetCyclicStatus = n.advertiseService("GetCyclicStatus", &Actuator_Services::GetCyclicStatus, &services_object); + + {% for function in list_function -%} + ros::ServiceServer service{{function}} = n.advertiseService("{{function}}", &Actuator_Services::{{function}}, &services_object); + {% endfor %} + + ROS_INFO("Node's services initialized correctly."); + + ros::Publisher pub_feedback = n.advertise("actuator_feedback", 1000); + ros::Publisher pub_joint_state = n.advertise("actuator_feedback/joint_state", 1000); + + kortex_actuator_driver::Feedback feedback; + kortex_actuator_driver::RefreshFeedback::Request req; + kortex_actuator_driver::RefreshFeedback::Response res; + + sensor_msgs::JointState joint_state; + + joint_state.position.resize(1); + joint_state.velocity.resize(1); + joint_state.effort.resize(1); + joint_state.name.resize(1); + + ros::Rate rate(cyclic_data_rate); // 100 hz + while (!ros::isShuttingDown()) + { + try + { + if(services_object.IsCyclicActive()) + { + services_object.RefreshFeedback(req, res); + + feedback.feedback_id = res.output.feedback_id; + + feedback.status_flags = res.output.status_flags; + feedback.jitter_comm = res.output.jitter_comm; + feedback.position = res.output.position; + feedback.velocity = res.output.velocity; + feedback.torque = res.output.torque; + feedback.current_motor = res.output.current_motor; + feedback.voltage = res.output.voltage; + feedback.temperature_motor = res.output.temperature_motor; + feedback.temperature_core = res.output.temperature_core; + feedback.fault_bank_a = res.output.fault_bank_a; + feedback.fault_bank_b = res.output.fault_bank_b; + feedback.warning_bank_a = res.output.warning_bank_a; + feedback.warning_bank_b = res.output.warning_bank_b; + + joint_state.header.stamp = ros::Time::now(); + joint_state.header.frame_id = std::to_string(res.output.feedback_id.identifier); + + joint_state.name[0] = "Actuator"; + joint_state.position[0] = TO_RAD(res.output.position); + joint_state.velocity[0] = TO_RAD(res.output.velocity); + joint_state.effort[0] = res.output.torque; + + pub_feedback.publish(feedback); + pub_joint_state.publish(joint_state); + } + } + + catch (KDetailedException& ex) + { + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + } + catch (std::runtime_error& ex2) + { + ROS_INFO("RUN TIME ERROR: %s\n", ex2.what()); + } + + ros::spinOnce(); + + rate.sleep(); + } + + return 1; +} diff --git a/kortex_actuator_driver/templates/proto_converter.cpp.jinja2 b/kortex_actuator_driver/templates/proto_converter.cpp.jinja2 new file mode 100644 index 00000000..a2e26763 --- /dev/null +++ b/kortex_actuator_driver/templates/proto_converter.cpp.jinja2 @@ -0,0 +1,77 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentFilename|lower}}_proto_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_proto_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToProtoData(kortex_actuator_driver::{{detailed_message.message.name}} input, {{detailed_message.message.name}} *output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") -%} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {%- if field.type == 11 %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + ToProtoData(input.{{field.name}}[i], output->add_{{field.name|lower}}()); + } + {%- else %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + output->add_{{field.name|lower}}(input.{{field.name}}[i]); + } + {% endif -%} + {% else -%} + {%- if field.type == 11 %} + ToProtoData(input.{{field.name}}, output->mutable_{{field.name}}()); + {%- elif field.type == 14 %}{# ENUM #} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output->set_{{field.name}}(({{field.type_name|replace(".", "", 1)|replace(".", "::")}})input.{{field.name}}); + {%- elif field.type == 12 %} + output->set_{{field.name}}(std::string(input.{{field.name}}.begin(), input.{{field.name}}.end())); + {%- else %} + output->set_{{field.name}}(input.{{field.name}}); + {%- endif -%} + {%- endif -%} + {%- endif -%} + {%- endfor -%} + + {% if detailed_message.HasOneOf == "true" %} + + {% for field in detailed_message.message.field %} + {%- if field.HasField("oneof_index") -%} + if(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.size() > 0) + { + {% if field.type == 11 -%} + ToProtoData(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0], output->mutable_{{field.name}}()); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} + output->set_{{field.name}}(({{list1[4]}})input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- else %} + output->set_{{field.name}}(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- endif %} + } + {% endif %} + {%- endfor -%} + {% endif %} + + return 0; +} +{% endfor %} diff --git a/kortex_actuator_driver/templates/proto_converter.h.jinja2 b/kortex_actuator_driver/templates/proto_converter.h.jinja2 new file mode 100644 index 00000000..049cc9a2 --- /dev/null +++ b/kortex_actuator_driver/templates/proto_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_actuator_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToProtoData(kortex_actuator_driver::{{detailed_message.message.name}} intput, {{detailed_message.message.name}} *output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/templates/ros_converter.cpp.jinja2 b/kortex_actuator_driver/templates/ros_converter.cpp.jinja2 new file mode 100644 index 00000000..cdc873f7 --- /dev/null +++ b/kortex_actuator_driver/templates/ros_converter.cpp.jinja2 @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentFilename|lower}}_ros_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_ros_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_actuator_driver::{{detailed_message.message.name}} &output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") %} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {% if field.type == 11 %} + {%- set splitTypeName = field.type_name.split('.') -%} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + gen3_actuator::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(i), temp); + output.{{field.name}}.push_back(temp); + } + {%- else %} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + output.{{field.name}}.push_back(input.{{field.name|lower}}(i)); + } + {%- endif %} + {%- else %} + {%- if field.type == 11 %} + ToRosData(input.{{field.name}}(), output.{{field.name}}); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output.{{field.name}} = input.{{field.name}}(); + {%- elif field.type == 12 %} + output.{{field.name}} = std::vector(input.{{field.name}}().begin(), input.{{field.name}}().end()); + {%- else %} + output.{{field.name}} = input.{{field.name}}(); + {%- endif %} + {%- endif %} + {%- endif %} + {%- endfor %} + + {% if detailed_message.HasOneOf == "true" %} + auto oneof_type = input.{{detailed_message.message.ListFields()[-1][1][0].name}}_case(); + + switch(oneof_type) + { + {%- for field in detailed_message.message.field -%} + {%- if field.HasField("oneof_index") -%} + {%- set splitTypeName = field.type_name.split('.') %} + {%- set EnumName = field.name.replace("_", " ").title().replace(" ", "") %} + case {{detailed_message.message.name}}::k{{EnumName}}: + { + {%- if field.type == 11 %} + gen3_actuator::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(), temp); + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(temp); + {%- elif field.type == 14 %} + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(input.{{field.name}}()); + {% endif %} + break; + } + {% endif %} + {%- endfor %} + } + {% endif -%} + + return 0; +} +{% endfor %} diff --git a/kortex_actuator_driver/templates/ros_converter.h.jinja2 b/kortex_actuator_driver/templates/ros_converter.h.jinja2 new file mode 100644 index 00000000..254292c2 --- /dev/null +++ b/kortex_actuator_driver/templates/ros_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_actuator_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_actuator_driver::{{detailed_message.message.name}} &output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_actuator_driver/templates/ros_enum.jinja2 b/kortex_actuator_driver/templates/ros_enum.jinja2 new file mode 100644 index 00000000..164146fc --- /dev/null +++ b/kortex_actuator_driver/templates/ros_enum.jinja2 @@ -0,0 +1,3 @@ +{% for member in item.value %} +uint32 {{member.name}} = {{member.number}} +{% endfor %} diff --git a/kortex_actuator_driver/templates/ros_message.jinja2 b/kortex_actuator_driver/templates/ros_message.jinja2 new file mode 100644 index 00000000..167f685f --- /dev/null +++ b/kortex_actuator_driver/templates/ros_message.jinja2 @@ -0,0 +1,44 @@ +{%- for member in item.field -%} +{%- if not member.HasField("oneof_index") -%} +{%- if member.type == 9 %} {# TYPE_STRING #} +string{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 12 %} {# TYPE_BYTES #} +uint8[] {{member.name}} +{%- elif member.type == 1 %} {# TYPE_DOUBLE #} +float64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 7 %} {# TYPE_FIXED32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 6 %} {# TYPE_FIXED64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 2 %} {# TYPE_FLOAT #} +float32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 5 %} {# TYPE_INT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 3 %} {# TYPE_INT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 15 %} {# TYPE_SFIXED32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 16 %} {# TYPE_SFIXED64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 17 %} {# TYPE_SINT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 18 %} {# TYPE_SINT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 13 %} {# TYPE_UINT32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 4 %} {# TYPE_UINT64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 14 -%} {# TYPE_ENUM #} +{% set list1 = member.type_name.split('.') %} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 8 %} {# TYPE_BOOL #} +bool {{member.name}} +{%- elif member.type == 11 %}{# TYPE MESSAGE #} +{% set list1 = member.type_name.split('.') %} +{{list1|last}}{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- endif -%} +{%- endif -%} +{%- endfor -%} +{%- if HasOneOf %} +{{item.name}}_{{item.ListFields()[-1][1][0].name}} oneof_{{item.ListFields()[-1][1][0].name}} +{%- endif -%} \ No newline at end of file diff --git a/kortex_actuator_driver/templates/ros_oneof.jinja2 b/kortex_actuator_driver/templates/ros_oneof.jinja2 new file mode 100644 index 00000000..4fac302a --- /dev/null +++ b/kortex_actuator_driver/templates/ros_oneof.jinja2 @@ -0,0 +1,9 @@ +{%- for member in item.field -%} +{% if member.HasField("oneof_index") %} +{% if member.type == 11 %} +{% set list1 = member.type_name.split('.') %}{{list1[4]}}[] {{member.name}} +{%- else -%} +uint32[] {{member.name}} +{%- endif -%} +{%- endif -%} +{% endfor %} \ No newline at end of file diff --git a/kortex_actuator_driver/templates/ros_service.jinja2 b/kortex_actuator_driver/templates/ros_service.jinja2 new file mode 100644 index 00000000..cc015cf8 --- /dev/null +++ b/kortex_actuator_driver/templates/ros_service.jinja2 @@ -0,0 +1,5 @@ +{% set split_input_type = item.input_type.split('.') %} +{%- set split_output_type = item.output_type.split('.') -%} +{{split_input_type[4]}} input +--- +{{split_output_type[4]}} output \ No newline at end of file diff --git a/kortex_api/include/.gitignore b/kortex_api/include/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_api/lib/.gitignore b/kortex_api/lib/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_api/readme.md b/kortex_api/readme.md new file mode 100644 index 00000000..681edf0b --- /dev/null +++ b/kortex_api/readme.md @@ -0,0 +1,39 @@ + +# Kortex API +This package contains all the files needed to use the c++ Kortex API which is used by all the driver nodes and the device manager. This package is mandatory because most of the other package refer to it. A more detailed documentation is available on the Kortex API [repository](https://github.com/Kinovarobotics/kortex). + + + +1. [Install Cpp Kortex Api & the needed dependencies](#install-cpp-kortex-api-the-needed-dependencies) +1. [Content](#content) + 1. [include](#include) + 1. [lib](#lib) + + + + +## Install Cpp Kortex Api & the needed dependencies + +> *Manual installation using downloaded archive:* +> + Download the archive via the [Kinova drive](https://drive.google.com/file/d/19zfCNlRUfNBbZoMW9LOpLjVrYOO2BwYb/view). +> + Uncompress the content of the archive and copy the content of /kortex_api/cpp/linux_x86/include/ and /kortex_api/cpp/linux_x86/lib/ respectively in kortex_api/include/ and kortex_api/lib/ . +> + Remove the folder /kortex_api/include/google/ + + + +## Content +The content of this package should not be deleted or changed. + +### include +Contains header files of the Kortex API. + +### lib +Contains binaries files of the Kortex API. diff --git a/kortex_description/CMakeLists.txt b/kortex_description/CMakeLists.txt new file mode 100644 index 00000000..bb671d14 --- /dev/null +++ b/kortex_description/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(kortex_description) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/kortex_description/config/joint_names_JACO3_URDF_V10.yaml b/kortex_description/config/joint_names_JACO3_URDF_V10.yaml new file mode 100644 index 00000000..39d5611d --- /dev/null +++ b/kortex_description/config/joint_names_JACO3_URDF_V10.yaml @@ -0,0 +1 @@ +controller_joint_names: ['Actuator1', 'Actuator2', 'Actuator3', 'Actuator4', 'Actuator5', 'Actuator6', 'Actuator7', ] diff --git a/kortex_description/launch/display.launch b/kortex_description/launch/display.launch new file mode 100644 index 00000000..c695a85d --- /dev/null +++ b/kortex_description/launch/display.launch @@ -0,0 +1,26 @@ + + + + + + + + + diff --git a/kortex_description/launch/gazebo.launch b/kortex_description/launch/gazebo.launch new file mode 100644 index 00000000..70ad31fa --- /dev/null +++ b/kortex_description/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + diff --git a/kortex_description/meshes/Bracelet_Link.STL b/kortex_description/meshes/Bracelet_Link.STL new file mode 100644 index 00000000..e8a9f58e Binary files /dev/null and b/kortex_description/meshes/Bracelet_Link.STL differ diff --git a/kortex_description/meshes/EndEffector_Link.STL b/kortex_description/meshes/EndEffector_Link.STL new file mode 100644 index 00000000..6dc3cfa9 Binary files /dev/null and b/kortex_description/meshes/EndEffector_Link.STL differ diff --git a/kortex_description/meshes/ForeArm_Link.STL b/kortex_description/meshes/ForeArm_Link.STL new file mode 100644 index 00000000..21ee2dbe Binary files /dev/null and b/kortex_description/meshes/ForeArm_Link.STL differ diff --git a/kortex_description/meshes/HalfArm1_Link.STL b/kortex_description/meshes/HalfArm1_Link.STL new file mode 100644 index 00000000..0d4d324c Binary files /dev/null and b/kortex_description/meshes/HalfArm1_Link.STL differ diff --git a/kortex_description/meshes/HalfArm2_Link.STL b/kortex_description/meshes/HalfArm2_Link.STL new file mode 100644 index 00000000..6d23022d Binary files /dev/null and b/kortex_description/meshes/HalfArm2_Link.STL differ diff --git a/kortex_description/meshes/Shoulder_Link.STL b/kortex_description/meshes/Shoulder_Link.STL new file mode 100644 index 00000000..6cfaedc1 Binary files /dev/null and b/kortex_description/meshes/Shoulder_Link.STL differ diff --git a/kortex_description/meshes/Wrist1_Link.STL b/kortex_description/meshes/Wrist1_Link.STL new file mode 100644 index 00000000..7ab8f92b Binary files /dev/null and b/kortex_description/meshes/Wrist1_Link.STL differ diff --git a/kortex_description/meshes/Wrist2_Link.STL b/kortex_description/meshes/Wrist2_Link.STL new file mode 100644 index 00000000..95bb979b Binary files /dev/null and b/kortex_description/meshes/Wrist2_Link.STL differ diff --git a/kortex_description/meshes/base_link.STL b/kortex_description/meshes/base_link.STL new file mode 100644 index 00000000..2b29c6a9 Binary files /dev/null and b/kortex_description/meshes/base_link.STL differ diff --git a/kortex_description/package.xml b/kortex_description/package.xml new file mode 100644 index 00000000..fe825e87 --- /dev/null +++ b/kortex_description/package.xml @@ -0,0 +1,21 @@ + + kortex_description + 1.0.0 + +

URDF Description package for ultra_lightweight

+

This package contains configuration data, 3D models and launch files +for ultra_lightweight robot

+ + KINOVA + + BSD + catkin + roslaunch + robot_state_publisher + rviz + joint_state_publisher + gazebo + + + + \ No newline at end of file diff --git a/kortex_description/readme.md b/kortex_description/readme.md new file mode 100644 index 00000000..9d38b9fe --- /dev/null +++ b/kortex_description/readme.md @@ -0,0 +1,15 @@ + +# Kortex Description +This package contains the URDF, with the STL files of a Gen3 robot. + + + + diff --git a/kortex_description/urdf/JACO3_URDF_V10.urdf b/kortex_description/urdf/JACO3_URDF_V10.urdf new file mode 100644 index 00000000..99df3ea6 --- /dev/null +++ b/kortex_description/urdf/JACO3_URDF_V10.urdf @@ -0,0 +1,494 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_device_manager/CMakeLists.txt b/kortex_device_manager/CMakeLists.txt new file mode 100644 index 00000000..cd5b2aaf --- /dev/null +++ b/kortex_device_manager/CMakeLists.txt @@ -0,0 +1,40 @@ + +cmake_minimum_required(VERSION 2.8.3) +project(kortex_device_manager) + +add_compile_options(-std=c++11) +add_definitions(-D_OS_UNIX) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) + +file(GLOB_RECURSE msg_list RELATIVE ${PROJECT_SOURCE_DIR}/msg "*.msg") +file(GLOB_RECURSE srv_list RELATIVE ${PROJECT_SOURCE_DIR}/srv "*.srv") +file(GLOB_RECURSE cpp_list RELATIVE ${PROJECT_SOURCE_DIR} "src/*.cpp") + +## Declare ROS messages and services +add_message_files(DIRECTORY msg FILES ${msg_list}) +add_service_files(DIRECTORY srv FILES ${srv_list}) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${PROJECT_SOURCE_DIR}/src) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client_stubs) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/messages) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/common) +include_directories(include ${PROJECT_SOURCE_DIR}/src/util) + +link_directories(${PROJECT_SOURCE_DIR}/../kortex_api/lib/release) + +add_executable(${PROJECT_NAME} ${cpp_list}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} CppKinovaApi gcov) + +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) + diff --git a/kortex_device_manager/RosGeneration.py b/kortex_device_manager/RosGeneration.py new file mode 100644 index 00000000..a93fc316 --- /dev/null +++ b/kortex_device_manager/RosGeneration.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python + +import sys + +from google.protobuf.compiler import plugin_pb2 as plugin +from google.protobuf import json_format as json_f + +import jinja2 + +import itertools +import json +import types +import os +import sys + +from google.protobuf.descriptor_pb2 import DescriptorProto, EnumDescriptorProto, ServiceDescriptorProto, FieldDescriptorProto, OneofDescriptorProto + +#Class that holds a protobuf message and some other details needed by the generator(jinja2 template). +class DetailedMessage: + def __init__(self, message=None): + self.message = message + self.HasOneOf = "false" + self.oneOfList = [] + +#Class that holds a protobuf service and some other details needed by the generator(jinja2 template). +class DetailedPackage: + def __init__(self, service=None): + self.name = "NoName" + self.service = service + +#JINJA2 function to render a file from a template. +def render(tpl_path, context): + path, filename = os.path.split(tpl_path) + return jinja2.Environment(loader=jinja2.FileSystemLoader(path or './')).get_template(filename).render(**context) + +#Main plugin function +def generate_code(request, response): + + #The context is the object sent to the JINJA2 template + context = types.SimpleNamespace() + context.serviceVersion = 1 + + context.detailedPackages = [] + + MainFilePath = os.path.join(".", "src/main.cpp") + function_list = [] + fileIndex = 0 + + for proto_file in request.proto_file: + context.detailedPackages.append(DetailedPackage()) + context.detailedPackages[fileIndex].name = proto_file.package.split(".")[-1] + context.detailedPackages[fileIndex].filename = proto_file.name.split(".")[0] + context.detailedPackages[fileIndex].namespace = proto_file.package.replace(".", "::") + context.detailedPackages[fileIndex].HasRPC = 0 + context.detailedPackages[fileIndex].HasMessage = 0 + + HeaderFilePath = os.path.join(".", "src/node.h") + CppFilePath = os.path.join(".", "src/node.cpp") + + #We lower the case to respect ROS coding standard style + CppProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.h".format(proto_file.name.split(".")[0].lower())) + CppRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.h".format(proto_file.name.split(".")[0].lower())) + + list_detailedMessage = [] + + # For every item in the current proto file + for item, package in traverse(proto_file): + context.HasOneOf = 0 + + + if isinstance(item, EnumDescriptorProto): + context.item = item + + ros_enumPath = os.path.join(".", "msg/{}.msg".format(item.name)) + + with open(ros_enumPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_enum.jinja2", context.__dict__)) + #If this it a message + if isinstance(item, DescriptorProto): + tempMessage = DetailedMessage(item) + + context.detailedPackages[fileIndex].HasMessage = 1 + + for member in item.field: + #If a member is part of a oneof, it will have this additional field + if member.HasField("oneof_index"): + context.HasOneOf = 1 + tempMessage.HasOneOf = "true" + else: + context.HasOneOf = 0 + tempMessage.HasOneOf = "false" + + context.item = item + + #If the proto file contains a ONEOF we need to generate a separate file to handle it. + if context.HasOneOf == 1: + + #This line gets the list of ONEOF that is in the current message. + oneOfList = item.ListFields()[-1][1] + + tempMessage.oneOfList = item.ListFields()[-1][1] + ros_oneofPath = os.path.join(".", "msg/{}_{}.msg".format(item.name, oneOfList[0].name)) + + with open(ros_oneofPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_oneof.jinja2", context.__dict__)) + + + list_detailedMessage.append(tempMessage) + ros_messagePath = os.path.join(".", "msg/{}.msg".format(item.name)) + + #We call jinja2 to generate a ROS message. + with open(ros_messagePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_message.jinja2", context.__dict__)) + + #If this is a service (A group of method) + if isinstance(item, ServiceDescriptorProto): + for method in item.method: + context.item = method + if "Topic" not in method.name: + function_list.append(method.name) + ros_servicePath = os.path.join(".", "srv/{}.srv".format(method.name)) + with open(ros_servicePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_service.jinja2", context.__dict__)) + + context.detailedPackages[fileIndex].service = item + context.detailedPackages[fileIndex].HasRPC = 1 + + context.currentPackageName = context.detailedPackages[fileIndex].name + context.currentNamespace = proto_file.package.replace(".", "::") + context.currentFilename = context.detailedPackages[fileIndex].filename + context.item = list_detailedMessage + + if context.detailedPackages[fileIndex].HasMessage == 1: + #Wecall jinja2 to generate a prot/ROS converter for every protobuf message. + with open(CppProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.cpp.jinja2", context.__dict__)) + with open(HeaderProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.h.jinja2", context.__dict__)) + with open(CppRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.cpp.jinja2", context.__dict__)) + with open(HeaderRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.h.jinja2", context.__dict__)) + + fileIndex = fileIndex + 1 + + context.list_function = function_list + + #We jinja2 to generate the ROS node. + with open(HeaderFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.h.jinja2", context.__dict__)) + with open(CppFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.cpp.jinja2", context.__dict__)) + with open(MainFilePath, 'wt') as mainFile: + mainFile.write(render("./templates/main.jinja2", context.__dict__)) + +def traverse(proto_file): + #recursive function that browse a protobof item + def _traverse(package, items): + for item in items: + yield item, package + + if isinstance(item, DescriptorProto): + for enum in item.enum_type: + yield enum, package + + for nested in item.nested_type: + nested_package = package + item.name + + for nested_item in _traverse(nested, nested_package): + yield nested_item, nested_package + if isinstance(item, ServiceDescriptorProto): + for rpc in item.method: + yield rpc, package + + #return a list of everything found in the proto file + return itertools.chain( + _traverse(proto_file.package, proto_file.enum_type), + _traverse(proto_file.package, proto_file.message_type), + _traverse(proto_file.package, proto_file.service), + ) + +if __name__ == '__main__': + # Read request message from stdin + data = sys.stdin.buffer.read() + + # Parse request + request = plugin.CodeGeneratorRequest() + request.ParseFromString(data) + + # Create response + response = plugin.CodeGeneratorResponse() + + # Generate code + generate_code(request, response) + + # Serialise response message + output = response.SerializeToString() + + # Write to stdout + sys.stdout.buffer.write(output) \ No newline at end of file diff --git a/kortex_device_manager/build/.gitignore b/kortex_device_manager/build/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_device_manager/kortex_device_manager.sh b/kortex_device_manager/kortex_device_manager.sh new file mode 100755 index 00000000..011eee96 --- /dev/null +++ b/kortex_device_manager/kortex_device_manager.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +python3 -u RosGeneration.py + diff --git a/kortex_device_manager/msg/ArmState.msg b/kortex_device_manager/msg/ArmState.msg new file mode 100644 index 00000000..d85bd69d --- /dev/null +++ b/kortex_device_manager/msg/ArmState.msg @@ -0,0 +1,22 @@ + +uint32 UNSPECIFIED_ARM_STATE = 0 + +uint32 BASE_INITIALIZATION = 1 + +uint32 IDLE = 2 + +uint32 ARM_INITIALIZATION = 3 + +uint32 ARM_IN_FAULT = 4 + +uint32 ARM_MAINTENANCE = 5 + +uint32 ARM_SERVOING_LOW_LEVEL = 6 + +uint32 ARM_SERVOING_READY = 7 + +uint32 ARM_SERVOING_PLAYING_SEQUENCE = 8 + +uint32 ARM_SERVOING_MANUALLY_CONTROLLED = 9 + +uint32 RESERVED = 255 diff --git a/kortex_device_manager/msg/BootloaderVersion.msg b/kortex_device_manager/msg/BootloaderVersion.msg new file mode 100644 index 00000000..d6dc8352 --- /dev/null +++ b/kortex_device_manager/msg/BootloaderVersion.msg @@ -0,0 +1,2 @@ + +uint32 bootloader_version \ No newline at end of file diff --git a/kortex_device_manager/msg/Connection.msg b/kortex_device_manager/msg/Connection.msg new file mode 100644 index 00000000..ca08b6c8 --- /dev/null +++ b/kortex_device_manager/msg/Connection.msg @@ -0,0 +1,5 @@ + + +UserProfileHandle user_handle +string connection_information +uint32 connection_identifier \ No newline at end of file diff --git a/kortex_device_manager/msg/DeviceHandle.msg b/kortex_device_manager/msg/DeviceHandle.msg new file mode 100644 index 00000000..49f84366 --- /dev/null +++ b/kortex_device_manager/msg/DeviceHandle.msg @@ -0,0 +1,5 @@ + + +uint32 device_type +uint32 device_identifier +uint32 order \ No newline at end of file diff --git a/kortex_device_manager/msg/DeviceHandles.msg b/kortex_device_manager/msg/DeviceHandles.msg new file mode 100644 index 00000000..739f4ff3 --- /dev/null +++ b/kortex_device_manager/msg/DeviceHandles.msg @@ -0,0 +1,3 @@ + + +DeviceHandle[] device_handle \ No newline at end of file diff --git a/kortex_device_manager/msg/DeviceType.msg b/kortex_device_manager/msg/DeviceType.msg new file mode 100644 index 00000000..b4dd97c2 --- /dev/null +++ b/kortex_device_manager/msg/DeviceType.msg @@ -0,0 +1,3 @@ + + +uint32 device_type \ No newline at end of file diff --git a/kortex_device_manager/msg/DeviceTypes.msg b/kortex_device_manager/msg/DeviceTypes.msg new file mode 100644 index 00000000..5a55df07 --- /dev/null +++ b/kortex_device_manager/msg/DeviceTypes.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_DEVICE_TYPE = 0 + +uint32 BASE = 1 + +uint32 VISION = 2 + +uint32 BIG_ACTUATOR = 3 + +uint32 SMALL_ACTUATOR = 4 + +uint32 INTERCONNECT = 5 + +uint32 GRIPPER = 6 diff --git a/kortex_device_manager/msg/Empty.msg b/kortex_device_manager/msg/Empty.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_device_manager/msg/FirmwareVersion.msg b/kortex_device_manager/msg/FirmwareVersion.msg new file mode 100644 index 00000000..ef44f90b --- /dev/null +++ b/kortex_device_manager/msg/FirmwareVersion.msg @@ -0,0 +1,2 @@ + +uint32 firmware_version \ No newline at end of file diff --git a/kortex_device_manager/msg/IPv4Settings.msg b/kortex_device_manager/msg/IPv4Settings.msg new file mode 100644 index 00000000..5be19d99 --- /dev/null +++ b/kortex_device_manager/msg/IPv4Settings.msg @@ -0,0 +1,4 @@ + +uint32 ipv4_address +uint32 ipv4_subnet_mask +uint32 ipv4_default_gateway \ No newline at end of file diff --git a/kortex_device_manager/msg/MACAddress.msg b/kortex_device_manager/msg/MACAddress.msg new file mode 100644 index 00000000..a2764fac --- /dev/null +++ b/kortex_device_manager/msg/MACAddress.msg @@ -0,0 +1,2 @@ + +uint8[] mac_address \ No newline at end of file diff --git a/kortex_device_manager/msg/ModelNumber.msg b/kortex_device_manager/msg/ModelNumber.msg new file mode 100644 index 00000000..adb82526 --- /dev/null +++ b/kortex_device_manager/msg/ModelNumber.msg @@ -0,0 +1,2 @@ + +string model_number \ No newline at end of file diff --git a/kortex_device_manager/msg/NotificationHandle.msg b/kortex_device_manager/msg/NotificationHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_device_manager/msg/NotificationHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_device_manager/msg/NotificationOptions.msg b/kortex_device_manager/msg/NotificationOptions.msg new file mode 100644 index 00000000..6ff46842 --- /dev/null +++ b/kortex_device_manager/msg/NotificationOptions.msg @@ -0,0 +1,5 @@ + + +uint32 type +uint32 rate_m_sec +float32 threshold_value \ No newline at end of file diff --git a/kortex_device_manager/msg/NotificationType.msg b/kortex_device_manager/msg/NotificationType.msg new file mode 100644 index 00000000..79dd058c --- /dev/null +++ b/kortex_device_manager/msg/NotificationType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_NOTIFICATION_TYPE = 0 + +uint32 THRESHOLD = 1 + +uint32 FIX_RATE = 2 + +uint32 EVENT = 3 diff --git a/kortex_device_manager/msg/PartNumber.msg b/kortex_device_manager/msg/PartNumber.msg new file mode 100644 index 00000000..0cd6468f --- /dev/null +++ b/kortex_device_manager/msg/PartNumber.msg @@ -0,0 +1,2 @@ + +string part_number \ No newline at end of file diff --git a/kortex_device_manager/msg/PartNumberRevision.msg b/kortex_device_manager/msg/PartNumberRevision.msg new file mode 100644 index 00000000..90cb8f65 --- /dev/null +++ b/kortex_device_manager/msg/PartNumberRevision.msg @@ -0,0 +1,2 @@ + +string part_number_revision \ No newline at end of file diff --git a/kortex_device_manager/msg/Permission.msg b/kortex_device_manager/msg/Permission.msg new file mode 100644 index 00000000..c5399e9c --- /dev/null +++ b/kortex_device_manager/msg/Permission.msg @@ -0,0 +1,8 @@ + +uint32 NO_PERMISSION = 0 + +uint32 READ_PERMISSION = 1 + +uint32 UPDATE_PERMISSION = 2 + +uint32 DELETE_PERMISSION = 4 diff --git a/kortex_device_manager/msg/PowerOnSelfTestResult.msg b/kortex_device_manager/msg/PowerOnSelfTestResult.msg new file mode 100644 index 00000000..53a0cef1 --- /dev/null +++ b/kortex_device_manager/msg/PowerOnSelfTestResult.msg @@ -0,0 +1,2 @@ + +uint32 power_on_self_test_result \ No newline at end of file diff --git a/kortex_device_manager/msg/RebootRqst.msg b/kortex_device_manager/msg/RebootRqst.msg new file mode 100644 index 00000000..befd3fc3 --- /dev/null +++ b/kortex_device_manager/msg/RebootRqst.msg @@ -0,0 +1,2 @@ + +uint32 delay \ No newline at end of file diff --git a/kortex_device_manager/msg/RunMode.msg b/kortex_device_manager/msg/RunMode.msg new file mode 100644 index 00000000..7b3b1a3a --- /dev/null +++ b/kortex_device_manager/msg/RunMode.msg @@ -0,0 +1,3 @@ + + +uint32 run_mode \ No newline at end of file diff --git a/kortex_device_manager/msg/RunModes.msg b/kortex_device_manager/msg/RunModes.msg new file mode 100644 index 00000000..15c2a7da --- /dev/null +++ b/kortex_device_manager/msg/RunModes.msg @@ -0,0 +1,10 @@ + +uint32 RUN_MODE = 0 + +uint32 CALIBRATION_MODE = 1 + +uint32 CONFIGURATION_MODE = 2 + +uint32 DEBUG_MODE = 3 + +uint32 TUNING_MODE = 4 diff --git a/kortex_device_manager/msg/SafetyConfiguration.msg b/kortex_device_manager/msg/SafetyConfiguration.msg new file mode 100644 index 00000000..6f1f0819 --- /dev/null +++ b/kortex_device_manager/msg/SafetyConfiguration.msg @@ -0,0 +1,7 @@ + + +SafetyHandle handle +float32 error_threshold +float32 warning_threshold + +SafetyEnable enable \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyConfigurationList.msg b/kortex_device_manager/msg/SafetyConfigurationList.msg new file mode 100644 index 00000000..8ae23b3d --- /dev/null +++ b/kortex_device_manager/msg/SafetyConfigurationList.msg @@ -0,0 +1,3 @@ + + +SafetyConfiguration[] configuration \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyEnable.msg b/kortex_device_manager/msg/SafetyEnable.msg new file mode 100644 index 00000000..771f7400 --- /dev/null +++ b/kortex_device_manager/msg/SafetyEnable.msg @@ -0,0 +1,4 @@ + + +SafetyHandle handle +bool enable \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyHandle.msg b/kortex_device_manager/msg/SafetyHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_device_manager/msg/SafetyHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyInformation.msg b/kortex_device_manager/msg/SafetyInformation.msg new file mode 100644 index 00000000..66be9094 --- /dev/null +++ b/kortex_device_manager/msg/SafetyInformation.msg @@ -0,0 +1,16 @@ + + +SafetyHandle handle +bool can_change_safety_state +bool has_warning_threshold +bool has_error_threshold + +uint32 limit_type +float32 default_warning_threshold +float32 default_error_threshold +float32 upper_hard_limit +float32 lower_hard_limit + +uint32 status + +uint32 unit \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyInformationList.msg b/kortex_device_manager/msg/SafetyInformationList.msg new file mode 100644 index 00000000..ff11edbe --- /dev/null +++ b/kortex_device_manager/msg/SafetyInformationList.msg @@ -0,0 +1,3 @@ + + +SafetyInformation[] information \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyLimitType.msg b/kortex_device_manager/msg/SafetyLimitType.msg new file mode 100644 index 00000000..f0beacf4 --- /dev/null +++ b/kortex_device_manager/msg/SafetyLimitType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_SAFETY_LIMIT_TYPE = 0 + +uint32 MINIMAL_LIMIT = 1 + +uint32 MAXIMAL_LIMIT = 2 + +uint32 EVENT_LIMIT = 3 diff --git a/kortex_device_manager/msg/SafetyNotification.msg b/kortex_device_manager/msg/SafetyNotification.msg new file mode 100644 index 00000000..36d88894 --- /dev/null +++ b/kortex_device_manager/msg/SafetyNotification.msg @@ -0,0 +1,11 @@ + + +SafetyHandle safety_handle + +uint32 value + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyStatus.msg b/kortex_device_manager/msg/SafetyStatus.msg new file mode 100644 index 00000000..b82ac221 --- /dev/null +++ b/kortex_device_manager/msg/SafetyStatus.msg @@ -0,0 +1,3 @@ + + +uint32 value \ No newline at end of file diff --git a/kortex_device_manager/msg/SafetyStatusValue.msg b/kortex_device_manager/msg/SafetyStatusValue.msg new file mode 100644 index 00000000..2ea80554 --- /dev/null +++ b/kortex_device_manager/msg/SafetyStatusValue.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED = 0 + +uint32 WARNING = 1 + +uint32 ERROR = 2 + +uint32 NORMAL = 3 diff --git a/kortex_device_manager/msg/SafetyThreshold.msg b/kortex_device_manager/msg/SafetyThreshold.msg new file mode 100644 index 00000000..b50b25e3 --- /dev/null +++ b/kortex_device_manager/msg/SafetyThreshold.msg @@ -0,0 +1,4 @@ + + +SafetyHandle handle +float32 value \ No newline at end of file diff --git a/kortex_device_manager/msg/SerialNumber.msg b/kortex_device_manager/msg/SerialNumber.msg new file mode 100644 index 00000000..9831a3ee --- /dev/null +++ b/kortex_device_manager/msg/SerialNumber.msg @@ -0,0 +1,2 @@ + +string serial_number \ No newline at end of file diff --git a/kortex_device_manager/msg/ServiceVersion.msg b/kortex_device_manager/msg/ServiceVersion.msg new file mode 100644 index 00000000..9665d1c2 --- /dev/null +++ b/kortex_device_manager/msg/ServiceVersion.msg @@ -0,0 +1,4 @@ + +uint32 RESERVED_0 = 0 + +uint32 CURRENT_VERSION = 1 diff --git a/kortex_device_manager/msg/Timestamp.msg b/kortex_device_manager/msg/Timestamp.msg new file mode 100644 index 00000000..5e60508c --- /dev/null +++ b/kortex_device_manager/msg/Timestamp.msg @@ -0,0 +1,3 @@ + +uint32 sec +uint32 usec \ No newline at end of file diff --git a/kortex_device_manager/msg/Unit.msg b/kortex_device_manager/msg/Unit.msg new file mode 100644 index 00000000..c019aeba --- /dev/null +++ b/kortex_device_manager/msg/Unit.msg @@ -0,0 +1,28 @@ + +uint32 UNSPECIFIED_UNIT = 0 + +uint32 CELSIUS = 1 + +uint32 AMPERE = 2 + +uint32 VOLT = 3 + +uint32 METER_PER_SECOND = 4 + +uint32 DEGREE_PER_SECOND = 5 + +uint32 METER_PER_SECOND_2 = 6 + +uint32 DEGREE_PER_SECOND_2 = 7 + +uint32 NEWTON = 8 + +uint32 NEWTON_METER = 9 + +uint32 KILOGRAM = 10 + +uint32 DEGREE = 11 + +uint32 TICK = 12 + +uint32 DEGREE_PER_MILLISECOND = 13 diff --git a/kortex_device_manager/msg/UserProfileHandle.msg b/kortex_device_manager/msg/UserProfileHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_device_manager/msg/UserProfileHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_device_manager/msg/non_generated/KortexError.msg b/kortex_device_manager/msg/non_generated/KortexError.msg new file mode 100644 index 00000000..45392615 --- /dev/null +++ b/kortex_device_manager/msg/non_generated/KortexError.msg @@ -0,0 +1,3 @@ +string description +uint32 code +uint32 subCode \ No newline at end of file diff --git a/kortex_device_manager/package.xml b/kortex_device_manager/package.xml new file mode 100644 index 00000000..9c848b67 --- /dev/null +++ b/kortex_device_manager/package.xml @@ -0,0 +1,30 @@ + + + kortex_device_manager + 1.0.0 + The kortex package that act as a device manager. + + KINOVA + + BSD + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_generation + message_runtime + + + + + + + + diff --git a/kortex_device_manager/protos/Common.options b/kortex_device_manager/protos/Common.options new file mode 100644 index 00000000..5f3020ab --- /dev/null +++ b/kortex_device_manager/protos/Common.options @@ -0,0 +1,4 @@ +Kinova.Api.Common.DeviceTypes long_names:false +Kinova.Api.Common.SafetyStatusValue long_names:false +Kinova.Api.Common.NotificationType long_names:false +Kinova.Api.Common.Unit long_names:false \ No newline at end of file diff --git a/kortex_device_manager/protos/Common.proto b/kortex_device_manager/protos/Common.proto new file mode 100644 index 00000000..50f762ec --- /dev/null +++ b/kortex_device_manager/protos/Common.proto @@ -0,0 +1,152 @@ +syntax = "proto3"; + +package Kinova.Api.Common; + +/** + * list of possible device types + */ +enum DeviceTypes { + UNSPECIFIED_DEVICE_TYPE = 0; + BASE = 1; + VISION = 2; + BIG_ACTUATOR = 3; + SMALL_ACTUATOR = 4; + INTERCONNECT = 5; + GRIPPER = 6; +} + +/** +* Message contains information about a device - device type, device identifier, and the order of the device within the robot +*/ +message DeviceHandle { + DeviceTypes device_type = 1; + uint32 device_identifier = 2; // Unique device identifier (used with other services) + uint32 order = 3; // Unique value indicating the order of that device versus the others to facilitate representation +} + +/** + * list of possible safety statuses + */ +enum SafetyStatusValue { + UNSPECIFIED = 0; + WARNING = 1; //warning safety reached + ERROR = 2; //error safety reached + NORMAL = 3; //safety is off +} + +/** + * Enumeration used as bitfields wih permission field + */ +enum Permission { + NO_PERMISSION = 0; + READ_PERMISSION = 1; //refers to a user's capability to read the entity + UPDATE_PERMISSION = 2;//refers to a user's capability to write or modify the entity + DELETE_PERMISSION = 4; //refers to a user's capability to delete the entity +} + +/** + * list of notification types + */ +enum NotificationType { + UNSPECIFIED_NOTIFICATION_TYPE = 0; + THRESHOLD = 1; + FIX_RATE = 2; + EVENT = 3; //Event type. Only this one is supported for now +} + +/** + * list of units used throughout API methods + */ +enum Unit { + UNSPECIFIED_UNIT = 0; + CELSIUS = 1; + AMPERE = 2; + VOLT = 3; + METER_PER_SECOND = 4; + DEGREE_PER_SECOND = 5; + METER_PER_SECOND_2 = 6; + DEGREE_PER_SECOND_2 = 7; + NEWTON = 8; + NEWTON_METER =9; + KILOGRAM = 10; + DEGREE = 11; + TICK = 12; + DEGREE_PER_MILLISECOND = 13; +} + +/** + * Message used when no information needs to be exchanged between client application and robot, and vice versa + */ +message Empty { +} + +/** + * Notification options + */ +message NotificationOptions { + NotificationType type = 1; //type of notification + uint32 rate_m_sec = 2; + float threshold_value = 3; +} + +/** + * Handle to a safety + */ +message SafetyHandle { + uint32 identifier = 1; +} + +/** + * Handle to a notification + */ +message NotificationHandle { + uint32 identifier = 1; +} + +/** + * Message that contains a Safety event + */ +message SafetyNotification { + SafetyHandle safety_handle = 1; //safety handle + SafetyStatusValue value = 2; //new safety status + Timestamp timestamp = 3; //event timestamp + UserProfileHandle user_handle = 4; //user that caused the safety event + Connection connection = 5; // connection that caused the safety event +} + +/** + * Timestamp based on epoch + */ +message Timestamp { + uint32 sec = 1; //epoch in seconds since 1970 + uint32 usec = 2;//microseconds after the second (0-999999) +} + +/** + * Handle to an existing User Profile. + */ +message UserProfileHandle { + uint32 identifier = 1; //User profile identifier + fixed32 permission = 2; //must use 'Permission' as bitwise +} + +message Connection { + UserProfileHandle user_handle = 1; //user profile handle, or set to zero if no user logged in + string connection_information = 2; //connection info (ex. IP address with port number) + uint32 connection_identifier = 3; //connection identifier +} + +enum ArmState +{ + UNSPECIFIED_ARM_STATE = 0; + BASE_INITIALIZATION = 1; // Cannot be reported as the Base initialization must be completed before allowing user connection + IDLE = 2; + ARM_INITIALIZATION = 3; + ARM_IN_FAULT = 4; + ARM_MAINTENANCE = 5; + ARM_SERVOING_LOW_LEVEL = 6; + ARM_SERVOING_READY = 7; + ARM_SERVOING_PLAYING_SEQUENCE = 8; + ARM_SERVOING_MANUALLY_CONTROLLED = 9; + RESERVED = 255; // For debugging, this state must never be reported outside the base. this means that a state is not mapped correctly +} \ No newline at end of file diff --git a/kortex_device_manager/protos/DeviceConfig.options b/kortex_device_manager/protos/DeviceConfig.options new file mode 100644 index 00000000..8dba0376 --- /dev/null +++ b/kortex_device_manager/protos/DeviceConfig.options @@ -0,0 +1,7 @@ +Kinova.Api.DeviceConfig.ModelNumber.model_number max_size:25 +Kinova.Api.DeviceConfig.PartNumber.part_number max_size:25 +Kinova.Api.DeviceConfig.SerialNumber.serial_number max_size:25 +Kinova.Api.DeviceConfig.PartNumberRevision.part_number_revision max_size:9 +Kinova.Api.DeviceConfig.MACAddress.mac_address max_size:6 fixed_length:true +Kinova.Api.DeviceConfig.RunModes long_names:false +Kinova.Api.DeviceConfig.SafetyLimitType long_names:false \ No newline at end of file diff --git a/kortex_device_manager/protos/DeviceConfig.proto b/kortex_device_manager/protos/DeviceConfig.proto new file mode 100644 index 00000000..d9a28047 --- /dev/null +++ b/kortex_device_manager/protos/DeviceConfig.proto @@ -0,0 +1,271 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.DeviceConfig; + +/** +* Service to get and set device configuration information +*/ +service DeviceConfig {//@PROXY_ID=9 @ERROR=Kinova.Api.Error + + // Returns the run mode for the device + rpc GetRunMode (Kinova.Api.Common.Empty) returns (RunMode); //@RPC_ID=1 + + // Sets the run mode for the device + rpc SetRunMode (RunMode) returns (Kinova.Api.Common.Empty); //@RPC_ID=2 + + // Returns the type for the device + rpc GetDeviceType (Kinova.Api.Common.Empty) returns (DeviceType); //@RPC_ID=3 + + // Returns the device firmware version + rpc GetFirmwareVersion (Kinova.Api.Common.Empty) returns (FirmwareVersion); //@RPC_ID=4 + + // Returns the device bootloader version + rpc GetBootloaderVersion (Kinova.Api.Common.Empty) returns (BootloaderVersion); //@RPC_ID=5 + + // Returns the device model number + rpc GetModelNumber (Kinova.Api.Common.Empty) returns (ModelNumber); //@RPC_ID=6 + + // Returns the device part number + rpc GetPartNumber (Kinova.Api.Common.Empty) returns (PartNumber); //@RPC_ID=7 + + // Returns the device serial number + rpc GetSerialNumber (Kinova.Api.Common.Empty) returns (SerialNumber); //@RPC_ID=8 + + // Returns the device MAC address + rpc GetMACAddress (Kinova.Api.Common.Empty) returns (MACAddress); //@RPC_ID=9 + + // Returns the device IPv4 settings + rpc GetIPv4Settings (Kinova.Api.Common.Empty) returns (IPv4Settings); //@RPC_ID=10 + + // Configures the device IPv4 settings + rpc SetIPv4Settings (IPv4Settings) returns (Kinova.Api.Common.Empty); //@RPC_ID=11 + + // Returns the device part number revision + rpc GetPartNumberRevision (Kinova.Api.Common.Empty) returns (PartNumberRevision); //@RPC_ID=12 + + // Returns the result on the device power on self test + rpc GetPowerOnSelfTestResult (Kinova.Api.Common.Empty) returns (PowerOnSelfTestResult); //@RPC_ID=13 + + // Sends a request to the device to reboot + rpc RebootRequest (RebootRqst) returns (Kinova.Api.Common.Empty); //@RPC_ID=14 + + //Enables (disable) the specified safety + rpc SetSafetyEnable (SafetyEnable) returns (Kinova.Api.Common.Empty); //@RPC_ID=15 + + //Sets the Error threshold for the specified safety + rpc SetSafetyErrorThreshold (SafetyThreshold) returns (Kinova.Api.Common.Empty); //@RPC_ID=16 + + //Sets the Warning threshold for the specified safety + rpc SetSafetyWarningThreshold (SafetyThreshold) returns (Kinova.Api.Common.Empty); //@RPC_ID=17 + + //Configures the specified safety (i.e. sets error and warning thresholds) + rpc SetSafetyConfiguration (SafetyConfiguration) returns (Kinova.Api.Common.Empty); //@RPC_ID=18 + + //Retrieves configuration about the specified safety + rpc GetSafetyConfiguration (Kinova.Api.Common.SafetyHandle) returns (SafetyConfiguration); //@RPC_ID=19 + + //Retrieves information about the specified safety + rpc GetSafetyInformation (Kinova.Api.Common.SafetyHandle) returns (SafetyInformation); //@RPC_ID=20 + + //Indicates if specified safety is enabled (or disabled) + rpc GetSafetyEnable (Kinova.Api.Common.SafetyHandle) returns (SafetyEnable); //@RPC_ID=21 + + //Indicates if the specified safety is raised + rpc GetSafetyStatus (Kinova.Api.Common.SafetyHandle) returns (SafetyStatus); //@RPC_ID=22 + + //Clear all safety status for this device if they are no longer raised + rpc ClearAllSafetyStatus (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=23 + + //Clear a specific safety status if it is no longer raised + rpc ClearSafetyStatus (Kinova.Api.Common.SafetyHandle) returns (Kinova.Api.Common.Empty); //@RPC_ID=24 + + //Gets configuration on every safeties + rpc GetAllSafetyConfiguration (Kinova.Api.Common.Empty) returns (SafetyConfigurationList); //@RPC_ID=25 + + //Gets information on every safeties + rpc GetAllSafetyInformation (Kinova.Api.Common.Empty) returns (SafetyInformationList); //@RPC_ID=26 + + //Puts back all safety configuration to factory defaults + rpc ResetSafetyDefaults (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty); //@RPC_ID=27 + + //Subscribes to safety notifications + rpc SafetyTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle); //@RPC_ID=28 @PUB_SUB=Kinova.Api.Common.SafetyNotification + + // Sets the device model number (intented for Kinova Production) + rpc SetModelNumber (ModelNumber) returns (Kinova.Api.Common.Empty); //@RPC_ID=29 + + // Sets the device part number (intented for Kinova Production) + rpc SetPartNumber (PartNumber) returns (Kinova.Api.Common.Empty); //@RPC_ID=30 + + // Sets the device part number revision (intented for Kinova Production) + rpc SetPartNumberRevision (PartNumberRevision) returns (Kinova.Api.Common.Empty); //@RPC_ID=31 + + // Sets the device serial number (intented for Kinova Production) + rpc SetSerialNumber (SerialNumber) returns (Kinova.Api.Common.Empty); //@RPC_ID=32 + + // Sets the device MAC address (intented for Kinova Production) + rpc SetMACAddress (MACAddress) returns (Kinova.Api.Common.Empty); //@RPC_ID=33 +} + +enum ServiceVersion { + RESERVED_0 = 0; + CURRENT_VERSION = 1; // Current Version +} + +/** +* Options for the run mode for the device +*/ +enum RunModes { + RUN_MODE = 0; + CALIBRATION_MODE = 1; // calibration mode + CONFIGURATION_MODE = 2; // configuration mode + DEBUG_MODE = 3; // debug mode + TUNING_MODE = 4; // tuning mode +} + +/** +* Message specifying the device type +*/ +message DeviceType { + Kinova.Api.Common.DeviceTypes device_type = 1; // Device type +} + + +/** +* Message specifying the run mode +*/ +message RunMode { + RunModes run_mode = 1; // Run mode +} + +/** +* Message specifying the firmware version for the device +*/ +message FirmwareVersion { + uint32 firmware_version = 1; // Firmware version +} + +/** +* Message specifying the bootloader version for the device +*/ +message BootloaderVersion { + uint32 bootloader_version = 1; // Bootloader version +} + +/** +* Message specifying the model number for the device +*/ +message ModelNumber { + string model_number = 1; // Model number of size 25 including null character +} + +/** +* String specifiying the part number for the device +*/ +message PartNumber { + string part_number = 1; // Part number of size 25 including null character +} + +/** +* String specifying the serial number for the device +*/ +message SerialNumber { + string serial_number = 1; // Serial number of size 25 including null character +} + +/** +* MAC address for the device +*/ +message MACAddress { + bytes mac_address = 1; // MAC address +} + +/** +* Message containing the IPv4 settings for the device, including address, subnet mask, and default gateway +*/ +message IPv4Settings { + uint32 ipv4_address = 1; // IPv4Address + uint32 ipv4_subnet_mask = 2; // IPv4SubnetMask + uint32 ipv4_default_gateway = 3; // IPv4DefaultGateway +} + +/** +* String specifying part number revision for the device +*/ +message PartNumberRevision { + string part_number_revision = 1; // Part number revision +} + +/** +* Result of power on self test +*/ +message PowerOnSelfTestResult { + uint32 power_on_self_test_result = 1; // Power on self test result +} + +/** +* Reboot request with bootloader delay +*/ +message RebootRqst { + uint32 delay = 1; // Bootloader delay +} + +/** + * Types of safeties limits + */ +enum SafetyLimitType { + UNSPECIFIED_SAFETY_LIMIT_TYPE = 0; + MINIMAL_LIMIT = 1; // Safety that will kick in below a certain Minimum threshold (ex. Minimum temperature safety) + MAXIMAL_LIMIT = 2; // Safety that will kick in above a certain Maximum threshold (ex. Maximum voltage safety) + EVENT_LIMIT = 3; // Safety that will kick in in reaction to a specific event (ex. motor drive fault) + +} + +/** + * Information about a particular safety + */ +message SafetyInformation { + Kinova.Api.Common.SafetyHandle handle = 1; // safety handle that this information is about + bool can_change_safety_state = 2; // true if related safety configuration can be modified + bool has_warning_threshold = 3; // true if safety status can go in Warning + bool has_error_threshold = 4; // true if safety status can go in Error + SafetyLimitType limit_type = 5; // safety limit type + float default_warning_threshold = 6; // default warning threshold (if 'limit_type' is either MINIMAL_LIMIT or MAXIMAL_LIMIT and 'has_warning_threshold' is true) + float default_error_threshold = 7; // default error threshold (if 'limit_type' is either MINIMAL_LIMIT or MAXIMAL_LIMIT and 'has_error_threshold' is true) + float upper_hard_limit = 8; // maximal threshold value (if 'limit_type' is either MINIMAL_LIMIT or MAXIMAL_LIMIT) + float lower_hard_limit = 9; // minimal threshold value (if 'limit_type' is either MINIMAL_LIMIT or MAXIMAL_LIMIT) + Kinova.Api.Common.SafetyStatusValue status = 11;// current Safety status + Kinova.Api.Common.Unit unit = 12; // Unit that the safety status is in +} + +message SafetyInformationList { + repeated SafetyInformation information = 1; +} + +message SafetyEnable { + Kinova.Api.Common.SafetyHandle handle = 1; + bool enable = 2; +} + +message SafetyThreshold { + Kinova.Api.Common.SafetyHandle handle = 1; + float value = 2; +} + +message SafetyConfiguration { + Kinova.Api.Common.SafetyHandle handle = 1; + float error_threshold = 2; + float warning_threshold = 3; + SafetyEnable enable = 4; +} + +message SafetyConfigurationList { + repeated SafetyConfiguration configuration = 1; +} + +message SafetyStatus { + Kinova.Api.Common.SafetyStatusValue value = 1; +} + diff --git a/kortex_device_manager/protos/DeviceManager.proto b/kortex_device_manager/protos/DeviceManager.proto new file mode 100644 index 00000000..abdf4e2a --- /dev/null +++ b/kortex_device_manager/protos/DeviceManager.proto @@ -0,0 +1,27 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.DeviceManager; + +/**************************************************************** + * This service is always present on every framework 3 robot + * Its purpose is to provide information about presence of devices + *****************************************************************/ +service DeviceManager {//@PROXY_ID=23 @ERROR=Kinova.Api.Error + + //Retrieves the list of every device that the robot contains, along with its type and order within the robot + rpc ReadAllDevices (Kinova.Api.Common.Empty) returns (DeviceHandles);//@RPC_ID=1 +} + +enum ServiceVersion { + RESERVED_0 = 0; + CURRENT_VERSION = 1; // Current Version +} + +/** +*Message containing a list of DeviceHandle objects +*/ +message DeviceHandles { + repeated Kinova.Api.Common.DeviceHandle device_handle = 1; +} diff --git a/kortex_device_manager/readme.md b/kortex_device_manager/readme.md new file mode 100644 index 00000000..c8a6c275 --- /dev/null +++ b/kortex_device_manager/readme.md @@ -0,0 +1,96 @@ + +# Kortex Device Manager + + + +1. [Content](#content) + 1. [build](#build) + 1. [msg](#msg) + 1. [non_generated](#non_generated) + 1. [protos](#protos) + 1. [src](#src) + 1. [srv](#srv) + 1. [templates](#templates) +1. [How to start the node](#how-to-start-the-node) +1. [Generation](#generation) + + + + +## Content + +### build +This folder's only purpose is to exist as a temp folder during the generation. It should not be used. + +### msg +This folder contains every custom message used by the node **kortex\_device\_manager**. All the .msg files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .msg files used by the node **kortex\_device\_manager**. + +| MSG | Description | +|:---:|:---:| +| KortexError.msg | Describe the topic /KortexError. Every service call of the node kortex_driver will publish in /KortexError everytime the Kortex API returns an error. | + + +### protos +This folder contains the protobuf files from where the MSG, SRV and sources files are generated. The content of this folder should not be modified. + +### src +This folder contains all the generated source files needed to build the node. The content of this folder should not be modified. + +### srv +This folder contains every custom services used by the node **kortex\_device\_manager**. All the .srv files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + + +### templates +This folder contains all the JINJA2 files needed by the protoc generator. For more details on the generation process, see the [Generation](#generation) section. + +| JINJA2 files | Description | +|:---:|:---:| +| main.jinja2 | Use to generate src/main.cpp | +| NodeServices.cpp.jinja2 | Use to generate src/node.cpp | +| NodeServices.h.jinja2 | Use to generate src/node.h | +| proto_converterCPP.jinja2 | Use to generate every src/*_proto\_converter.cpp files | +| proto_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_converterCPP.jinja2 | Use to generate every src/*_ros\_converter.cpp files | +| ros_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_enum.jinja2 | Use to generate every msg/*.msg files that represent a protobuf enum | +| ros_message.jinja2 | Use to generate every msg/*.msg files that represent a protobuf message | +| ros_oneof.jinja2 | Use to generate every msg/*.msg files that represent a protobuf oneof | +| ros_service.jinja2 | Use to generate every msg/*.msg files that represent a protobuf RPC | + + +## How to start the node + +rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10 + +In the command above, you would be running the kortex\_device\_manager node on a Gen3 robot with an IP address of 192.168.1.10 + + +## Generation +

+The generation process is based on a custom protobuf plugin. Basically, most of the generation process is in the python file RosGeneration.py located at the package's root folder. Before launching the generation make sure you have the python module Jinja2 installed on your computer. +

+ +To launch the generation of this package: + +1. Open a terminal window. +1. Browse the root of this package [YOUR\_ROS\_WORKSPACE]/src/ros\_kortex/kortex\_device\_manager/ +1. Make sure that the file kortex_driver.sh can be executed. If not then chmod +x kortex_device_manager.sh +1. Run this command: protoc --plugin=protoc-gen-custom=kortex_device_manager.sh -I./protos/ --custom_out=./build ./protos/\*.prot +1. The result of the generation should be on thos folder: + * /src + * /msg + * /srv + diff --git a/kortex_device_manager/src/common_proto_converter.cpp b/kortex_device_manager/src/common_proto_converter.cpp new file mode 100644 index 00000000..f6bcd03f --- /dev/null +++ b/kortex_device_manager/src/common_proto_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_device_manager::DeviceHandle input, DeviceHandle *output) +{ + output->set_device_type((Kinova::Api::Common::DeviceTypes)input.device_type); + output->set_device_identifier(input.device_identifier); + output->set_order(input.order); + + return 0; +} +int ToProtoData(kortex_device_manager::Empty input, Empty *output) +{ + + return 0; +} +int ToProtoData(kortex_device_manager::NotificationOptions input, NotificationOptions *output) +{ + output->set_type((Kinova::Api::Common::NotificationType)input.type); + output->set_rate_m_sec(input.rate_m_sec); + output->set_threshold_value(input.threshold_value); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyHandle input, SafetyHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_device_manager::NotificationHandle input, NotificationHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyNotification input, SafetyNotification *output) +{ + ToProtoData(input.safety_handle, output->mutable_safety_handle()); + output->set_value((Kinova::Api::Common::SafetyStatusValue)input.value); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_device_manager::Timestamp input, Timestamp *output) +{ + output->set_sec(input.sec); + output->set_usec(input.usec); + + return 0; +} +int ToProtoData(kortex_device_manager::UserProfileHandle input, UserProfileHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_device_manager::Connection input, Connection *output) +{ + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_connection_information(input.connection_information); + output->set_connection_identifier(input.connection_identifier); + + return 0; +} diff --git a/kortex_device_manager/src/common_proto_converter.h b/kortex_device_manager/src/common_proto_converter.h new file mode 100644 index 00000000..c16d8738 --- /dev/null +++ b/kortex_device_manager/src/common_proto_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonPROTO_CONVERTER_H_ +#define _KORTEX_CommonPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceHandle.h" +#include "kortex_device_manager/Empty.h" +#include "kortex_device_manager/NotificationOptions.h" +#include "kortex_device_manager/SafetyHandle.h" +#include "kortex_device_manager/NotificationHandle.h" +#include "kortex_device_manager/SafetyNotification.h" +#include "kortex_device_manager/Timestamp.h" +#include "kortex_device_manager/UserProfileHandle.h" +#include "kortex_device_manager/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToProtoData(kortex_device_manager::DeviceHandle intput, DeviceHandle *output); +int ToProtoData(kortex_device_manager::Empty intput, Empty *output); +int ToProtoData(kortex_device_manager::NotificationOptions intput, NotificationOptions *output); +int ToProtoData(kortex_device_manager::SafetyHandle intput, SafetyHandle *output); +int ToProtoData(kortex_device_manager::NotificationHandle intput, NotificationHandle *output); +int ToProtoData(kortex_device_manager::SafetyNotification intput, SafetyNotification *output); +int ToProtoData(kortex_device_manager::Timestamp intput, Timestamp *output); +int ToProtoData(kortex_device_manager::UserProfileHandle intput, UserProfileHandle *output); +int ToProtoData(kortex_device_manager::Connection intput, Connection *output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/common_ros_converter.cpp b/kortex_device_manager/src/common_ros_converter.cpp new file mode 100644 index 00000000..bdfd6e33 --- /dev/null +++ b/kortex_device_manager/src/common_ros_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_ros_converter.h" + + +int ToRosData(DeviceHandle input, kortex_device_manager::DeviceHandle &output) +{ + output.device_type = input.device_type(); + output.device_identifier = input.device_identifier(); + output.order = input.order(); + + return 0; +} +int ToRosData(Empty input, kortex_device_manager::Empty &output) +{ + + return 0; +} +int ToRosData(NotificationOptions input, kortex_device_manager::NotificationOptions &output) +{ + output.type = input.type(); + output.rate_m_sec = input.rate_m_sec(); + output.threshold_value = input.threshold_value(); + + return 0; +} +int ToRosData(SafetyHandle input, kortex_device_manager::SafetyHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(NotificationHandle input, kortex_device_manager::NotificationHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(SafetyNotification input, kortex_device_manager::SafetyNotification &output) +{ + ToRosData(input.safety_handle(), output.safety_handle); + output.value = input.value(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(Timestamp input, kortex_device_manager::Timestamp &output) +{ + output.sec = input.sec(); + output.usec = input.usec(); + + return 0; +} +int ToRosData(UserProfileHandle input, kortex_device_manager::UserProfileHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(Connection input, kortex_device_manager::Connection &output) +{ + ToRosData(input.user_handle(), output.user_handle); + output.connection_information = input.connection_information(); + output.connection_identifier = input.connection_identifier(); + + return 0; +} diff --git a/kortex_device_manager/src/common_ros_converter.h b/kortex_device_manager/src/common_ros_converter.h new file mode 100644 index 00000000..164b3a3f --- /dev/null +++ b/kortex_device_manager/src/common_ros_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonROS_CONVERTER_H_ +#define _KORTEX_CommonROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceHandle.h" +#include "kortex_device_manager/Empty.h" +#include "kortex_device_manager/NotificationOptions.h" +#include "kortex_device_manager/SafetyHandle.h" +#include "kortex_device_manager/NotificationHandle.h" +#include "kortex_device_manager/SafetyNotification.h" +#include "kortex_device_manager/Timestamp.h" +#include "kortex_device_manager/UserProfileHandle.h" +#include "kortex_device_manager/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToRosData(DeviceHandle input, kortex_device_manager::DeviceHandle &output); +int ToRosData(Empty input, kortex_device_manager::Empty &output); +int ToRosData(NotificationOptions input, kortex_device_manager::NotificationOptions &output); +int ToRosData(SafetyHandle input, kortex_device_manager::SafetyHandle &output); +int ToRosData(NotificationHandle input, kortex_device_manager::NotificationHandle &output); +int ToRosData(SafetyNotification input, kortex_device_manager::SafetyNotification &output); +int ToRosData(Timestamp input, kortex_device_manager::Timestamp &output); +int ToRosData(UserProfileHandle input, kortex_device_manager::UserProfileHandle &output); +int ToRosData(Connection input, kortex_device_manager::Connection &output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/deviceconfig_proto_converter.cpp b/kortex_device_manager/src/deviceconfig_proto_converter.cpp new file mode 100644 index 00000000..a3a2d11b --- /dev/null +++ b/kortex_device_manager/src/deviceconfig_proto_converter.cpp @@ -0,0 +1,160 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "deviceconfig_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_device_manager::DeviceType input, DeviceType *output) +{ + output->set_device_type((Kinova::Api::Common::DeviceTypes)input.device_type); + + return 0; +} +int ToProtoData(kortex_device_manager::RunMode input, RunMode *output) +{ + output->set_run_mode((Kinova::Api::DeviceConfig::RunModes)input.run_mode); + + return 0; +} +int ToProtoData(kortex_device_manager::FirmwareVersion input, FirmwareVersion *output) +{ + output->set_firmware_version(input.firmware_version); + + return 0; +} +int ToProtoData(kortex_device_manager::BootloaderVersion input, BootloaderVersion *output) +{ + output->set_bootloader_version(input.bootloader_version); + + return 0; +} +int ToProtoData(kortex_device_manager::ModelNumber input, ModelNumber *output) +{ + output->set_model_number(input.model_number); + + return 0; +} +int ToProtoData(kortex_device_manager::PartNumber input, PartNumber *output) +{ + output->set_part_number(input.part_number); + + return 0; +} +int ToProtoData(kortex_device_manager::SerialNumber input, SerialNumber *output) +{ + output->set_serial_number(input.serial_number); + + return 0; +} +int ToProtoData(kortex_device_manager::MACAddress input, MACAddress *output) +{ + output->set_mac_address(std::string(input.mac_address.begin(), input.mac_address.end())); + + return 0; +} +int ToProtoData(kortex_device_manager::IPv4Settings input, IPv4Settings *output) +{ + output->set_ipv4_address(input.ipv4_address); + output->set_ipv4_subnet_mask(input.ipv4_subnet_mask); + output->set_ipv4_default_gateway(input.ipv4_default_gateway); + + return 0; +} +int ToProtoData(kortex_device_manager::PartNumberRevision input, PartNumberRevision *output) +{ + output->set_part_number_revision(input.part_number_revision); + + return 0; +} +int ToProtoData(kortex_device_manager::PowerOnSelfTestResult input, PowerOnSelfTestResult *output) +{ + output->set_power_on_self_test_result(input.power_on_self_test_result); + + return 0; +} +int ToProtoData(kortex_device_manager::RebootRqst input, RebootRqst *output) +{ + output->set_delay(input.delay); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyInformation input, SafetyInformation *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_can_change_safety_state(input.can_change_safety_state); + output->set_has_warning_threshold(input.has_warning_threshold); + output->set_has_error_threshold(input.has_error_threshold); + output->set_limit_type((Kinova::Api::DeviceConfig::SafetyLimitType)input.limit_type); + output->set_default_warning_threshold(input.default_warning_threshold); + output->set_default_error_threshold(input.default_error_threshold); + output->set_upper_hard_limit(input.upper_hard_limit); + output->set_lower_hard_limit(input.lower_hard_limit); + output->set_status((Kinova::Api::Common::SafetyStatusValue)input.status); + output->set_unit((Kinova::Api::Common::Unit)input.unit); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyInformationList input, SafetyInformationList *output) +{ + output->clear_information(); + for(int i = 0; i < input.information.size(); i++) + { + ToProtoData(input.information[i], output->add_information()); + } + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyEnable input, SafetyEnable *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_enable(input.enable); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyThreshold input, SafetyThreshold *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyConfiguration input, SafetyConfiguration *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_error_threshold(input.error_threshold); + output->set_warning_threshold(input.warning_threshold); + ToProtoData(input.enable, output->mutable_enable()); + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyConfigurationList input, SafetyConfigurationList *output) +{ + output->clear_configuration(); + for(int i = 0; i < input.configuration.size(); i++) + { + ToProtoData(input.configuration[i], output->add_configuration()); + } + + return 0; +} +int ToProtoData(kortex_device_manager::SafetyStatus input, SafetyStatus *output) +{ + output->set_value((Kinova::Api::Common::SafetyStatusValue)input.value); + + return 0; +} diff --git a/kortex_device_manager/src/deviceconfig_proto_converter.h b/kortex_device_manager/src/deviceconfig_proto_converter.h new file mode 100644 index 00000000..9be3f0ba --- /dev/null +++ b/kortex_device_manager/src/deviceconfig_proto_converter.h @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DeviceConfigPROTO_CONVERTER_H_ +#define _KORTEX_DeviceConfigPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceType.h" +#include "kortex_device_manager/RunMode.h" +#include "kortex_device_manager/FirmwareVersion.h" +#include "kortex_device_manager/BootloaderVersion.h" +#include "kortex_device_manager/ModelNumber.h" +#include "kortex_device_manager/PartNumber.h" +#include "kortex_device_manager/SerialNumber.h" +#include "kortex_device_manager/MACAddress.h" +#include "kortex_device_manager/IPv4Settings.h" +#include "kortex_device_manager/PartNumberRevision.h" +#include "kortex_device_manager/PowerOnSelfTestResult.h" +#include "kortex_device_manager/RebootRqst.h" +#include "kortex_device_manager/SafetyInformation.h" +#include "kortex_device_manager/SafetyInformationList.h" +#include "kortex_device_manager/SafetyEnable.h" +#include "kortex_device_manager/SafetyThreshold.h" +#include "kortex_device_manager/SafetyConfiguration.h" +#include "kortex_device_manager/SafetyConfigurationList.h" +#include "kortex_device_manager/SafetyStatus.h" + + +using namespace Kinova::Api::DeviceConfig; + +int ToProtoData(kortex_device_manager::DeviceType intput, DeviceType *output); +int ToProtoData(kortex_device_manager::RunMode intput, RunMode *output); +int ToProtoData(kortex_device_manager::FirmwareVersion intput, FirmwareVersion *output); +int ToProtoData(kortex_device_manager::BootloaderVersion intput, BootloaderVersion *output); +int ToProtoData(kortex_device_manager::ModelNumber intput, ModelNumber *output); +int ToProtoData(kortex_device_manager::PartNumber intput, PartNumber *output); +int ToProtoData(kortex_device_manager::SerialNumber intput, SerialNumber *output); +int ToProtoData(kortex_device_manager::MACAddress intput, MACAddress *output); +int ToProtoData(kortex_device_manager::IPv4Settings intput, IPv4Settings *output); +int ToProtoData(kortex_device_manager::PartNumberRevision intput, PartNumberRevision *output); +int ToProtoData(kortex_device_manager::PowerOnSelfTestResult intput, PowerOnSelfTestResult *output); +int ToProtoData(kortex_device_manager::RebootRqst intput, RebootRqst *output); +int ToProtoData(kortex_device_manager::SafetyInformation intput, SafetyInformation *output); +int ToProtoData(kortex_device_manager::SafetyInformationList intput, SafetyInformationList *output); +int ToProtoData(kortex_device_manager::SafetyEnable intput, SafetyEnable *output); +int ToProtoData(kortex_device_manager::SafetyThreshold intput, SafetyThreshold *output); +int ToProtoData(kortex_device_manager::SafetyConfiguration intput, SafetyConfiguration *output); +int ToProtoData(kortex_device_manager::SafetyConfigurationList intput, SafetyConfigurationList *output); +int ToProtoData(kortex_device_manager::SafetyStatus intput, SafetyStatus *output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/deviceconfig_ros_converter.cpp b/kortex_device_manager/src/deviceconfig_ros_converter.cpp new file mode 100644 index 00000000..1fefb387 --- /dev/null +++ b/kortex_device_manager/src/deviceconfig_ros_converter.cpp @@ -0,0 +1,164 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "deviceconfig_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(DeviceType input, kortex_device_manager::DeviceType &output) +{ + output.device_type = input.device_type(); + + return 0; +} +int ToRosData(RunMode input, kortex_device_manager::RunMode &output) +{ + output.run_mode = input.run_mode(); + + return 0; +} +int ToRosData(FirmwareVersion input, kortex_device_manager::FirmwareVersion &output) +{ + output.firmware_version = input.firmware_version(); + + return 0; +} +int ToRosData(BootloaderVersion input, kortex_device_manager::BootloaderVersion &output) +{ + output.bootloader_version = input.bootloader_version(); + + return 0; +} +int ToRosData(ModelNumber input, kortex_device_manager::ModelNumber &output) +{ + output.model_number = input.model_number(); + + return 0; +} +int ToRosData(PartNumber input, kortex_device_manager::PartNumber &output) +{ + output.part_number = input.part_number(); + + return 0; +} +int ToRosData(SerialNumber input, kortex_device_manager::SerialNumber &output) +{ + output.serial_number = input.serial_number(); + + return 0; +} +int ToRosData(MACAddress input, kortex_device_manager::MACAddress &output) +{ + output.mac_address = std::vector(input.mac_address().begin(), input.mac_address().end()); + + return 0; +} +int ToRosData(IPv4Settings input, kortex_device_manager::IPv4Settings &output) +{ + output.ipv4_address = input.ipv4_address(); + output.ipv4_subnet_mask = input.ipv4_subnet_mask(); + output.ipv4_default_gateway = input.ipv4_default_gateway(); + + return 0; +} +int ToRosData(PartNumberRevision input, kortex_device_manager::PartNumberRevision &output) +{ + output.part_number_revision = input.part_number_revision(); + + return 0; +} +int ToRosData(PowerOnSelfTestResult input, kortex_device_manager::PowerOnSelfTestResult &output) +{ + output.power_on_self_test_result = input.power_on_self_test_result(); + + return 0; +} +int ToRosData(RebootRqst input, kortex_device_manager::RebootRqst &output) +{ + output.delay = input.delay(); + + return 0; +} +int ToRosData(SafetyInformation input, kortex_device_manager::SafetyInformation &output) +{ + ToRosData(input.handle(), output.handle); + output.can_change_safety_state = input.can_change_safety_state(); + output.has_warning_threshold = input.has_warning_threshold(); + output.has_error_threshold = input.has_error_threshold(); + output.limit_type = input.limit_type(); + output.default_warning_threshold = input.default_warning_threshold(); + output.default_error_threshold = input.default_error_threshold(); + output.upper_hard_limit = input.upper_hard_limit(); + output.lower_hard_limit = input.lower_hard_limit(); + output.status = input.status(); + output.unit = input.unit(); + + return 0; +} +int ToRosData(SafetyInformationList input, kortex_device_manager::SafetyInformationList &output) +{ + output.information.clear(); + for(int i = 0; i < input.information_size(); i++) + { + kortex_device_manager::SafetyInformation temp; + ToRosData(input.information(i), temp); + output.information.push_back(temp); + } + + return 0; +} +int ToRosData(SafetyEnable input, kortex_device_manager::SafetyEnable &output) +{ + ToRosData(input.handle(), output.handle); + output.enable = input.enable(); + + return 0; +} +int ToRosData(SafetyThreshold input, kortex_device_manager::SafetyThreshold &output) +{ + ToRosData(input.handle(), output.handle); + output.value = input.value(); + + return 0; +} +int ToRosData(SafetyConfiguration input, kortex_device_manager::SafetyConfiguration &output) +{ + ToRosData(input.handle(), output.handle); + output.error_threshold = input.error_threshold(); + output.warning_threshold = input.warning_threshold(); + ToRosData(input.enable(), output.enable); + + return 0; +} +int ToRosData(SafetyConfigurationList input, kortex_device_manager::SafetyConfigurationList &output) +{ + output.configuration.clear(); + for(int i = 0; i < input.configuration_size(); i++) + { + kortex_device_manager::SafetyConfiguration temp; + ToRosData(input.configuration(i), temp); + output.configuration.push_back(temp); + } + + return 0; +} +int ToRosData(SafetyStatus input, kortex_device_manager::SafetyStatus &output) +{ + output.value = input.value(); + + return 0; +} diff --git a/kortex_device_manager/src/deviceconfig_ros_converter.h b/kortex_device_manager/src/deviceconfig_ros_converter.h new file mode 100644 index 00000000..cc1632c7 --- /dev/null +++ b/kortex_device_manager/src/deviceconfig_ros_converter.h @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DeviceConfigROS_CONVERTER_H_ +#define _KORTEX_DeviceConfigROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceType.h" +#include "kortex_device_manager/RunMode.h" +#include "kortex_device_manager/FirmwareVersion.h" +#include "kortex_device_manager/BootloaderVersion.h" +#include "kortex_device_manager/ModelNumber.h" +#include "kortex_device_manager/PartNumber.h" +#include "kortex_device_manager/SerialNumber.h" +#include "kortex_device_manager/MACAddress.h" +#include "kortex_device_manager/IPv4Settings.h" +#include "kortex_device_manager/PartNumberRevision.h" +#include "kortex_device_manager/PowerOnSelfTestResult.h" +#include "kortex_device_manager/RebootRqst.h" +#include "kortex_device_manager/SafetyInformation.h" +#include "kortex_device_manager/SafetyInformationList.h" +#include "kortex_device_manager/SafetyEnable.h" +#include "kortex_device_manager/SafetyThreshold.h" +#include "kortex_device_manager/SafetyConfiguration.h" +#include "kortex_device_manager/SafetyConfigurationList.h" +#include "kortex_device_manager/SafetyStatus.h" + + +using namespace Kinova::Api::DeviceConfig; + +int ToRosData(DeviceType input, kortex_device_manager::DeviceType &output); +int ToRosData(RunMode input, kortex_device_manager::RunMode &output); +int ToRosData(FirmwareVersion input, kortex_device_manager::FirmwareVersion &output); +int ToRosData(BootloaderVersion input, kortex_device_manager::BootloaderVersion &output); +int ToRosData(ModelNumber input, kortex_device_manager::ModelNumber &output); +int ToRosData(PartNumber input, kortex_device_manager::PartNumber &output); +int ToRosData(SerialNumber input, kortex_device_manager::SerialNumber &output); +int ToRosData(MACAddress input, kortex_device_manager::MACAddress &output); +int ToRosData(IPv4Settings input, kortex_device_manager::IPv4Settings &output); +int ToRosData(PartNumberRevision input, kortex_device_manager::PartNumberRevision &output); +int ToRosData(PowerOnSelfTestResult input, kortex_device_manager::PowerOnSelfTestResult &output); +int ToRosData(RebootRqst input, kortex_device_manager::RebootRqst &output); +int ToRosData(SafetyInformation input, kortex_device_manager::SafetyInformation &output); +int ToRosData(SafetyInformationList input, kortex_device_manager::SafetyInformationList &output); +int ToRosData(SafetyEnable input, kortex_device_manager::SafetyEnable &output); +int ToRosData(SafetyThreshold input, kortex_device_manager::SafetyThreshold &output); +int ToRosData(SafetyConfiguration input, kortex_device_manager::SafetyConfiguration &output); +int ToRosData(SafetyConfigurationList input, kortex_device_manager::SafetyConfigurationList &output); +int ToRosData(SafetyStatus input, kortex_device_manager::SafetyStatus &output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/devicemanager_proto_converter.cpp b/kortex_device_manager/src/devicemanager_proto_converter.cpp new file mode 100644 index 00000000..ec89c604 --- /dev/null +++ b/kortex_device_manager/src/devicemanager_proto_converter.cpp @@ -0,0 +1,31 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "devicemanager_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_device_manager::DeviceHandles input, DeviceHandles *output) +{ + output->clear_device_handle(); + for(int i = 0; i < input.device_handle.size(); i++) + { + ToProtoData(input.device_handle[i], output->add_device_handle()); + } + + return 0; +} diff --git a/kortex_device_manager/src/devicemanager_proto_converter.h b/kortex_device_manager/src/devicemanager_proto_converter.h new file mode 100644 index 00000000..d04d1eae --- /dev/null +++ b/kortex_device_manager/src/devicemanager_proto_converter.h @@ -0,0 +1,50 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DeviceManagerPROTO_CONVERTER_H_ +#define _KORTEX_DeviceManagerPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceHandles.h" + + +using namespace Kinova::Api::DeviceManager; + +int ToProtoData(kortex_device_manager::DeviceHandles intput, DeviceHandles *output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/devicemanager_ros_converter.cpp b/kortex_device_manager/src/devicemanager_ros_converter.cpp new file mode 100644 index 00000000..35027ed0 --- /dev/null +++ b/kortex_device_manager/src/devicemanager_ros_converter.cpp @@ -0,0 +1,33 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "devicemanager_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(DeviceHandles input, kortex_device_manager::DeviceHandles &output) +{ + output.device_handle.clear(); + for(int i = 0; i < input.device_handle_size(); i++) + { + kortex_device_manager::DeviceHandle temp; + ToRosData(input.device_handle(i), temp); + output.device_handle.push_back(temp); + } + + return 0; +} diff --git a/kortex_device_manager/src/devicemanager_ros_converter.h b/kortex_device_manager/src/devicemanager_ros_converter.h new file mode 100644 index 00000000..2d319260 --- /dev/null +++ b/kortex_device_manager/src/devicemanager_ros_converter.h @@ -0,0 +1,50 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_DeviceManagerROS_CONVERTER_H_ +#define _KORTEX_DeviceManagerROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_device_manager/DeviceHandles.h" + + +using namespace Kinova::Api::DeviceManager; + +int ToRosData(DeviceHandles input, kortex_device_manager::DeviceHandles &output); + +#endif \ No newline at end of file diff --git a/kortex_device_manager/src/main.cpp b/kortex_device_manager/src/main.cpp new file mode 100644 index 00000000..b472d598 --- /dev/null +++ b/kortex_device_manager/src/main.cpp @@ -0,0 +1,80 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "DeviceManager"); + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 1) + { + ROS_INFO("Connecting to IP = %s", argv[1]); + } + else + { + ROS_INFO("You need to provide an IP adresse as a parameter. ex: rosrun package node 192.168.1.1"); + ros::shutdown(); + return 0; + } + + KortexDeviceManager services_object(argv[1], n); + + ros::ServiceServer serviceGetRunMode = n.advertiseService("GetRunMode", &KortexDeviceManager::GetRunMode, &services_object); + ros::ServiceServer serviceSetRunMode = n.advertiseService("SetRunMode", &KortexDeviceManager::SetRunMode, &services_object); + ros::ServiceServer serviceGetDeviceType = n.advertiseService("GetDeviceType", &KortexDeviceManager::GetDeviceType, &services_object); + ros::ServiceServer serviceGetFirmwareVersion = n.advertiseService("GetFirmwareVersion", &KortexDeviceManager::GetFirmwareVersion, &services_object); + ros::ServiceServer serviceGetBootloaderVersion = n.advertiseService("GetBootloaderVersion", &KortexDeviceManager::GetBootloaderVersion, &services_object); + ros::ServiceServer serviceGetModelNumber = n.advertiseService("GetModelNumber", &KortexDeviceManager::GetModelNumber, &services_object); + ros::ServiceServer serviceGetPartNumber = n.advertiseService("GetPartNumber", &KortexDeviceManager::GetPartNumber, &services_object); + ros::ServiceServer serviceGetSerialNumber = n.advertiseService("GetSerialNumber", &KortexDeviceManager::GetSerialNumber, &services_object); + ros::ServiceServer serviceGetMACAddress = n.advertiseService("GetMACAddress", &KortexDeviceManager::GetMACAddress, &services_object); + ros::ServiceServer serviceGetIPv4Settings = n.advertiseService("GetIPv4Settings", &KortexDeviceManager::GetIPv4Settings, &services_object); + ros::ServiceServer serviceSetIPv4Settings = n.advertiseService("SetIPv4Settings", &KortexDeviceManager::SetIPv4Settings, &services_object); + ros::ServiceServer serviceGetPartNumberRevision = n.advertiseService("GetPartNumberRevision", &KortexDeviceManager::GetPartNumberRevision, &services_object); + ros::ServiceServer serviceGetPowerOnSelfTestResult = n.advertiseService("GetPowerOnSelfTestResult", &KortexDeviceManager::GetPowerOnSelfTestResult, &services_object); + ros::ServiceServer serviceRebootRequest = n.advertiseService("RebootRequest", &KortexDeviceManager::RebootRequest, &services_object); + ros::ServiceServer serviceSetSafetyEnable = n.advertiseService("SetSafetyEnable", &KortexDeviceManager::SetSafetyEnable, &services_object); + ros::ServiceServer serviceSetSafetyErrorThreshold = n.advertiseService("SetSafetyErrorThreshold", &KortexDeviceManager::SetSafetyErrorThreshold, &services_object); + ros::ServiceServer serviceSetSafetyWarningThreshold = n.advertiseService("SetSafetyWarningThreshold", &KortexDeviceManager::SetSafetyWarningThreshold, &services_object); + ros::ServiceServer serviceSetSafetyConfiguration = n.advertiseService("SetSafetyConfiguration", &KortexDeviceManager::SetSafetyConfiguration, &services_object); + ros::ServiceServer serviceGetSafetyConfiguration = n.advertiseService("GetSafetyConfiguration", &KortexDeviceManager::GetSafetyConfiguration, &services_object); + ros::ServiceServer serviceGetSafetyInformation = n.advertiseService("GetSafetyInformation", &KortexDeviceManager::GetSafetyInformation, &services_object); + ros::ServiceServer serviceGetSafetyEnable = n.advertiseService("GetSafetyEnable", &KortexDeviceManager::GetSafetyEnable, &services_object); + ros::ServiceServer serviceGetSafetyStatus = n.advertiseService("GetSafetyStatus", &KortexDeviceManager::GetSafetyStatus, &services_object); + ros::ServiceServer serviceClearAllSafetyStatus = n.advertiseService("ClearAllSafetyStatus", &KortexDeviceManager::ClearAllSafetyStatus, &services_object); + ros::ServiceServer serviceClearSafetyStatus = n.advertiseService("ClearSafetyStatus", &KortexDeviceManager::ClearSafetyStatus, &services_object); + ros::ServiceServer serviceGetAllSafetyConfiguration = n.advertiseService("GetAllSafetyConfiguration", &KortexDeviceManager::GetAllSafetyConfiguration, &services_object); + ros::ServiceServer serviceGetAllSafetyInformation = n.advertiseService("GetAllSafetyInformation", &KortexDeviceManager::GetAllSafetyInformation, &services_object); + ros::ServiceServer serviceResetSafetyDefaults = n.advertiseService("ResetSafetyDefaults", &KortexDeviceManager::ResetSafetyDefaults, &services_object); + ros::ServiceServer serviceSetModelNumber = n.advertiseService("SetModelNumber", &KortexDeviceManager::SetModelNumber, &services_object); + ros::ServiceServer serviceSetPartNumber = n.advertiseService("SetPartNumber", &KortexDeviceManager::SetPartNumber, &services_object); + ros::ServiceServer serviceSetPartNumberRevision = n.advertiseService("SetPartNumberRevision", &KortexDeviceManager::SetPartNumberRevision, &services_object); + ros::ServiceServer serviceSetSerialNumber = n.advertiseService("SetSerialNumber", &KortexDeviceManager::SetSerialNumber, &services_object); + ros::ServiceServer serviceSetMACAddress = n.advertiseService("SetMACAddress", &KortexDeviceManager::SetMACAddress, &services_object); + ros::ServiceServer serviceReadAllDevices = n.advertiseService("ReadAllDevices", &KortexDeviceManager::ReadAllDevices, &services_object); + + + ROS_INFO("Node's services initialized correctly."); + + ros::spin(); + + return 1; +} \ No newline at end of file diff --git a/kortex_device_manager/src/node.cpp b/kortex_device_manager/src/node.cpp new file mode 100644 index 00000000..8c94bf50 --- /dev/null +++ b/kortex_device_manager/src/node.cpp @@ -0,0 +1,978 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "common_ros_converter.h" +#include "common_proto_converter.h" +#include "deviceconfig_ros_converter.h" +#include "deviceconfig_proto_converter.h" +#include "devicemanager_ros_converter.h" +#include "devicemanager_proto_converter.h" +KortexDeviceManager::KortexDeviceManager(char* ip, ros::NodeHandle& n) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + + + m_deviceconfig = new DeviceConfig::DeviceConfigClient(m_router); + m_devicemanager = new DeviceManager::DeviceManagerClient(m_router);m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + std::cout << "\nSession Created\n"; + + m_pub_Error = m_n.advertise("KortexError", 1000); + m_pub_SafetyTopic = m_n.advertise("SafetyTopic", 1000);std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + + + + +bool KortexDeviceManager::GetRunMode(kortex_device_manager::GetRunMode::Request &req, kortex_device_manager::GetRunMode::Response &res) +{ + Empty input; + RunMode output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetRunMode(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::SetRunMode(kortex_device_manager::SetRunMode::Request &req, kortex_device_manager::SetRunMode::Response &res) +{ + RunMode input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetRunMode(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::GetDeviceType(kortex_device_manager::GetDeviceType::Request &req, kortex_device_manager::GetDeviceType::Response &res) +{ + Empty input; + DeviceType output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetDeviceType(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetFirmwareVersion(kortex_device_manager::GetFirmwareVersion::Request &req, kortex_device_manager::GetFirmwareVersion::Response &res) +{ + Empty input; + FirmwareVersion output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetFirmwareVersion(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetBootloaderVersion(kortex_device_manager::GetBootloaderVersion::Request &req, kortex_device_manager::GetBootloaderVersion::Response &res) +{ + Empty input; + BootloaderVersion output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetBootloaderVersion(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetModelNumber(kortex_device_manager::GetModelNumber::Request &req, kortex_device_manager::GetModelNumber::Response &res) +{ + Empty input; + ModelNumber output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetModelNumber(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetPartNumber(kortex_device_manager::GetPartNumber::Request &req, kortex_device_manager::GetPartNumber::Response &res) +{ + Empty input; + PartNumber output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetPartNumber(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetSerialNumber(kortex_device_manager::GetSerialNumber::Request &req, kortex_device_manager::GetSerialNumber::Response &res) +{ + Empty input; + SerialNumber output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetSerialNumber(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetMACAddress(kortex_device_manager::GetMACAddress::Request &req, kortex_device_manager::GetMACAddress::Response &res) +{ + Empty input; + MACAddress output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetMACAddress(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetIPv4Settings(kortex_device_manager::GetIPv4Settings::Request &req, kortex_device_manager::GetIPv4Settings::Response &res) +{ + Empty input; + IPv4Settings output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetIPv4Settings(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::SetIPv4Settings(kortex_device_manager::SetIPv4Settings::Request &req, kortex_device_manager::SetIPv4Settings::Response &res) +{ + IPv4Settings input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetIPv4Settings(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::GetPartNumberRevision(kortex_device_manager::GetPartNumberRevision::Request &req, kortex_device_manager::GetPartNumberRevision::Response &res) +{ + Empty input; + PartNumberRevision output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetPartNumberRevision(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetPowerOnSelfTestResult(kortex_device_manager::GetPowerOnSelfTestResult::Request &req, kortex_device_manager::GetPowerOnSelfTestResult::Response &res) +{ + Empty input; + PowerOnSelfTestResult output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetPowerOnSelfTestResult(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::RebootRequest(kortex_device_manager::RebootRequest::Request &req, kortex_device_manager::RebootRequest::Response &res) +{ + RebootRqst input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->RebootRequest(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetSafetyEnable(kortex_device_manager::SetSafetyEnable::Request &req, kortex_device_manager::SetSafetyEnable::Response &res) +{ + SafetyEnable input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetSafetyEnable(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetSafetyErrorThreshold(kortex_device_manager::SetSafetyErrorThreshold::Request &req, kortex_device_manager::SetSafetyErrorThreshold::Response &res) +{ + SafetyThreshold input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetSafetyErrorThreshold(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetSafetyWarningThreshold(kortex_device_manager::SetSafetyWarningThreshold::Request &req, kortex_device_manager::SetSafetyWarningThreshold::Response &res) +{ + SafetyThreshold input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetSafetyWarningThreshold(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetSafetyConfiguration(kortex_device_manager::SetSafetyConfiguration::Request &req, kortex_device_manager::SetSafetyConfiguration::Response &res) +{ + SafetyConfiguration input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetSafetyConfiguration(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::GetSafetyConfiguration(kortex_device_manager::GetSafetyConfiguration::Request &req, kortex_device_manager::GetSafetyConfiguration::Response &res) +{ + SafetyHandle input; + ToProtoData(req.input, &input); + SafetyConfiguration output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetSafetyConfiguration(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetSafetyInformation(kortex_device_manager::GetSafetyInformation::Request &req, kortex_device_manager::GetSafetyInformation::Response &res) +{ + SafetyHandle input; + ToProtoData(req.input, &input); + SafetyInformation output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetSafetyInformation(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetSafetyEnable(kortex_device_manager::GetSafetyEnable::Request &req, kortex_device_manager::GetSafetyEnable::Response &res) +{ + SafetyHandle input; + ToProtoData(req.input, &input); + SafetyEnable output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetSafetyEnable(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetSafetyStatus(kortex_device_manager::GetSafetyStatus::Request &req, kortex_device_manager::GetSafetyStatus::Response &res) +{ + SafetyHandle input; + ToProtoData(req.input, &input); + SafetyStatus output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetSafetyStatus(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::ClearAllSafetyStatus(kortex_device_manager::ClearAllSafetyStatus::Request &req, kortex_device_manager::ClearAllSafetyStatus::Response &res) +{ + Empty input; + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->ClearAllSafetyStatus(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::ClearSafetyStatus(kortex_device_manager::ClearSafetyStatus::Request &req, kortex_device_manager::ClearSafetyStatus::Response &res) +{ + SafetyHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->ClearSafetyStatus(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::GetAllSafetyConfiguration(kortex_device_manager::GetAllSafetyConfiguration::Request &req, kortex_device_manager::GetAllSafetyConfiguration::Response &res) +{ + Empty input; + SafetyConfigurationList output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetAllSafetyConfiguration(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::GetAllSafetyInformation(kortex_device_manager::GetAllSafetyInformation::Request &req, kortex_device_manager::GetAllSafetyInformation::Response &res) +{ + Empty input; + SafetyInformationList output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_deviceconfig->GetAllSafetyInformation(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool KortexDeviceManager::ResetSafetyDefaults(kortex_device_manager::ResetSafetyDefaults::Request &req, kortex_device_manager::ResetSafetyDefaults::Response &res) +{ + Empty input; + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->ResetSafetyDefaults(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::OnNotificationSafetyTopic(kortex_device_manager::SafetyTopic::Request &req, kortex_device_manager::SafetyTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_device_manager::KortexError result_error; + + try + { + + std::function< void (Kinova::Api::Common::SafetyNotification) > callback = std::bind(&KortexDeviceManager::cb_SafetyTopic, this, std::placeholders::_1); + output = m_deviceconfig->OnNotificationSafetyTopic(callback, input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void KortexDeviceManager::cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) +{ + kortex_device_manager::SafetyNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SafetyTopic.publish(ros_msg); +} + +bool KortexDeviceManager::SetModelNumber(kortex_device_manager::SetModelNumber::Request &req, kortex_device_manager::SetModelNumber::Response &res) +{ + ModelNumber input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetModelNumber(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetPartNumber(kortex_device_manager::SetPartNumber::Request &req, kortex_device_manager::SetPartNumber::Response &res) +{ + PartNumber input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetPartNumber(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetPartNumberRevision(kortex_device_manager::SetPartNumberRevision::Request &req, kortex_device_manager::SetPartNumberRevision::Response &res) +{ + PartNumberRevision input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetPartNumberRevision(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetSerialNumber(kortex_device_manager::SetSerialNumber::Request &req, kortex_device_manager::SetSerialNumber::Response &res) +{ + SerialNumber input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetSerialNumber(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool KortexDeviceManager::SetMACAddress(kortex_device_manager::SetMACAddress::Request &req, kortex_device_manager::SetMACAddress::Response &res) +{ + MACAddress input; + ToProtoData(req.input, &input); + Empty output; + kortex_device_manager::KortexError result_error; + + try + { + m_deviceconfig->SetMACAddress(input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + + +bool KortexDeviceManager::ReadAllDevices(kortex_device_manager::ReadAllDevices::Request &req, kortex_device_manager::ReadAllDevices::Response &res) +{ + Empty input; + DeviceHandles output; + kortex_device_manager::KortexError result_error; + + try + { + output = m_devicemanager->ReadAllDevices(); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_device_manager/src/node.h b/kortex_device_manager/src/node.h new file mode 100644 index 00000000..11164747 --- /dev/null +++ b/kortex_device_manager/src/node.h @@ -0,0 +1,142 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_SERVICES_H_ +#define _KORTEX_SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include "kortex_device_manager/GetRunMode.h" +#include "kortex_device_manager/SetRunMode.h" +#include "kortex_device_manager/GetDeviceType.h" +#include "kortex_device_manager/GetFirmwareVersion.h" +#include "kortex_device_manager/GetBootloaderVersion.h" +#include "kortex_device_manager/GetModelNumber.h" +#include "kortex_device_manager/GetPartNumber.h" +#include "kortex_device_manager/GetSerialNumber.h" +#include "kortex_device_manager/GetMACAddress.h" +#include "kortex_device_manager/GetIPv4Settings.h" +#include "kortex_device_manager/SetIPv4Settings.h" +#include "kortex_device_manager/GetPartNumberRevision.h" +#include "kortex_device_manager/GetPowerOnSelfTestResult.h" +#include "kortex_device_manager/RebootRequest.h" +#include "kortex_device_manager/SetSafetyEnable.h" +#include "kortex_device_manager/SetSafetyErrorThreshold.h" +#include "kortex_device_manager/SetSafetyWarningThreshold.h" +#include "kortex_device_manager/SetSafetyConfiguration.h" +#include "kortex_device_manager/GetSafetyConfiguration.h" +#include "kortex_device_manager/GetSafetyInformation.h" +#include "kortex_device_manager/GetSafetyEnable.h" +#include "kortex_device_manager/GetSafetyStatus.h" +#include "kortex_device_manager/ClearAllSafetyStatus.h" +#include "kortex_device_manager/ClearSafetyStatus.h" +#include "kortex_device_manager/GetAllSafetyConfiguration.h" +#include "kortex_device_manager/GetAllSafetyInformation.h" +#include "kortex_device_manager/ResetSafetyDefaults.h" +#include "kortex_device_manager/SafetyTopic.h" +#include "kortex_device_manager/SetModelNumber.h" +#include "kortex_device_manager/SetPartNumber.h" +#include "kortex_device_manager/SetPartNumberRevision.h" +#include "kortex_device_manager/SetSerialNumber.h" +#include "kortex_device_manager/SetMACAddress.h" +#include "kortex_device_manager/ReadAllDevices.h" +#include "kortex_device_manager/KortexError.h" + +using namespace std; +using namespace Kinova::Api; +using namespace Kinova::Api::Common; +using namespace Kinova::Api::DeviceConfig; +using namespace Kinova::Api::DeviceManager; + +class KortexDeviceManager +{ + public: + KortexDeviceManager(char* ip, ros::NodeHandle& n); + + + bool GetRunMode(kortex_device_manager::GetRunMode::Request &req, kortex_device_manager::GetRunMode::Response &res); + bool SetRunMode(kortex_device_manager::SetRunMode::Request &req, kortex_device_manager::SetRunMode::Response &res); + bool GetDeviceType(kortex_device_manager::GetDeviceType::Request &req, kortex_device_manager::GetDeviceType::Response &res); + bool GetFirmwareVersion(kortex_device_manager::GetFirmwareVersion::Request &req, kortex_device_manager::GetFirmwareVersion::Response &res); + bool GetBootloaderVersion(kortex_device_manager::GetBootloaderVersion::Request &req, kortex_device_manager::GetBootloaderVersion::Response &res); + bool GetModelNumber(kortex_device_manager::GetModelNumber::Request &req, kortex_device_manager::GetModelNumber::Response &res); + bool GetPartNumber(kortex_device_manager::GetPartNumber::Request &req, kortex_device_manager::GetPartNumber::Response &res); + bool GetSerialNumber(kortex_device_manager::GetSerialNumber::Request &req, kortex_device_manager::GetSerialNumber::Response &res); + bool GetMACAddress(kortex_device_manager::GetMACAddress::Request &req, kortex_device_manager::GetMACAddress::Response &res); + bool GetIPv4Settings(kortex_device_manager::GetIPv4Settings::Request &req, kortex_device_manager::GetIPv4Settings::Response &res); + bool SetIPv4Settings(kortex_device_manager::SetIPv4Settings::Request &req, kortex_device_manager::SetIPv4Settings::Response &res); + bool GetPartNumberRevision(kortex_device_manager::GetPartNumberRevision::Request &req, kortex_device_manager::GetPartNumberRevision::Response &res); + bool GetPowerOnSelfTestResult(kortex_device_manager::GetPowerOnSelfTestResult::Request &req, kortex_device_manager::GetPowerOnSelfTestResult::Response &res); + bool RebootRequest(kortex_device_manager::RebootRequest::Request &req, kortex_device_manager::RebootRequest::Response &res); + bool SetSafetyEnable(kortex_device_manager::SetSafetyEnable::Request &req, kortex_device_manager::SetSafetyEnable::Response &res); + bool SetSafetyErrorThreshold(kortex_device_manager::SetSafetyErrorThreshold::Request &req, kortex_device_manager::SetSafetyErrorThreshold::Response &res); + bool SetSafetyWarningThreshold(kortex_device_manager::SetSafetyWarningThreshold::Request &req, kortex_device_manager::SetSafetyWarningThreshold::Response &res); + bool SetSafetyConfiguration(kortex_device_manager::SetSafetyConfiguration::Request &req, kortex_device_manager::SetSafetyConfiguration::Response &res); + bool GetSafetyConfiguration(kortex_device_manager::GetSafetyConfiguration::Request &req, kortex_device_manager::GetSafetyConfiguration::Response &res); + bool GetSafetyInformation(kortex_device_manager::GetSafetyInformation::Request &req, kortex_device_manager::GetSafetyInformation::Response &res); + bool GetSafetyEnable(kortex_device_manager::GetSafetyEnable::Request &req, kortex_device_manager::GetSafetyEnable::Response &res); + bool GetSafetyStatus(kortex_device_manager::GetSafetyStatus::Request &req, kortex_device_manager::GetSafetyStatus::Response &res); + bool ClearAllSafetyStatus(kortex_device_manager::ClearAllSafetyStatus::Request &req, kortex_device_manager::ClearAllSafetyStatus::Response &res); + bool ClearSafetyStatus(kortex_device_manager::ClearSafetyStatus::Request &req, kortex_device_manager::ClearSafetyStatus::Response &res); + bool GetAllSafetyConfiguration(kortex_device_manager::GetAllSafetyConfiguration::Request &req, kortex_device_manager::GetAllSafetyConfiguration::Response &res); + bool GetAllSafetyInformation(kortex_device_manager::GetAllSafetyInformation::Request &req, kortex_device_manager::GetAllSafetyInformation::Response &res); + bool ResetSafetyDefaults(kortex_device_manager::ResetSafetyDefaults::Request &req, kortex_device_manager::ResetSafetyDefaults::Response &res); + bool OnNotificationSafetyTopic(kortex_device_manager::SafetyTopic::Request &req, kortex_device_manager::SafetyTopic::Response &res); + void cb_SafetyTopic(SafetyNotification notif); + bool SetModelNumber(kortex_device_manager::SetModelNumber::Request &req, kortex_device_manager::SetModelNumber::Response &res); + bool SetPartNumber(kortex_device_manager::SetPartNumber::Request &req, kortex_device_manager::SetPartNumber::Response &res); + bool SetPartNumberRevision(kortex_device_manager::SetPartNumberRevision::Request &req, kortex_device_manager::SetPartNumberRevision::Response &res); + bool SetSerialNumber(kortex_device_manager::SetSerialNumber::Request &req, kortex_device_manager::SetSerialNumber::Response &res); + bool SetMACAddress(kortex_device_manager::SetMACAddress::Request &req, kortex_device_manager::SetMACAddress::Response &res); + + bool ReadAllDevices(kortex_device_manager::ReadAllDevices::Request &req, kortex_device_manager::ReadAllDevices::Response &res); + + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + + DeviceConfigClient* m_deviceconfig; + DeviceManagerClient* m_devicemanager; + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_SafetyTopic; +}; +#endif diff --git a/kortex_device_manager/src/util/diagnostic.h b/kortex_device_manager/src/util/diagnostic.h new file mode 100644 index 00000000..f0199870 --- /dev/null +++ b/kortex_device_manager/src/util/diagnostic.h @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +struct kortex_error +{ + int error_code; + std::string description; +}; \ No newline at end of file diff --git a/kortex_device_manager/src/util/math_util.h b/kortex_device_manager/src/util/math_util.h new file mode 100644 index 00000000..bf935386 --- /dev/null +++ b/kortex_device_manager/src/util/math_util.h @@ -0,0 +1,12 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +#define TO_RAD(degree) degree * M_PI / 180.0 \ No newline at end of file diff --git a/kortex_device_manager/srv/ClearAllSafetyStatus.srv b/kortex_device_manager/srv/ClearAllSafetyStatus.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_device_manager/srv/ClearAllSafetyStatus.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/ClearSafetyStatus.srv b/kortex_device_manager/srv/ClearSafetyStatus.srv new file mode 100644 index 00000000..a44abf78 --- /dev/null +++ b/kortex_device_manager/srv/ClearSafetyStatus.srv @@ -0,0 +1,3 @@ +SafetyHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetAllSafetyConfiguration.srv b/kortex_device_manager/srv/GetAllSafetyConfiguration.srv new file mode 100644 index 00000000..f60360fc --- /dev/null +++ b/kortex_device_manager/srv/GetAllSafetyConfiguration.srv @@ -0,0 +1,3 @@ +Empty input +--- +SafetyConfigurationList output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetAllSafetyInformation.srv b/kortex_device_manager/srv/GetAllSafetyInformation.srv new file mode 100644 index 00000000..ccb6480c --- /dev/null +++ b/kortex_device_manager/srv/GetAllSafetyInformation.srv @@ -0,0 +1,3 @@ +Empty input +--- +SafetyInformationList output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetBootloaderVersion.srv b/kortex_device_manager/srv/GetBootloaderVersion.srv new file mode 100644 index 00000000..f9df34be --- /dev/null +++ b/kortex_device_manager/srv/GetBootloaderVersion.srv @@ -0,0 +1,3 @@ +Empty input +--- +BootloaderVersion output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetDeviceType.srv b/kortex_device_manager/srv/GetDeviceType.srv new file mode 100644 index 00000000..841cafb0 --- /dev/null +++ b/kortex_device_manager/srv/GetDeviceType.srv @@ -0,0 +1,3 @@ +Empty input +--- +DeviceType output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetFirmwareVersion.srv b/kortex_device_manager/srv/GetFirmwareVersion.srv new file mode 100644 index 00000000..f92f54c5 --- /dev/null +++ b/kortex_device_manager/srv/GetFirmwareVersion.srv @@ -0,0 +1,3 @@ +Empty input +--- +FirmwareVersion output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetIPv4Settings.srv b/kortex_device_manager/srv/GetIPv4Settings.srv new file mode 100644 index 00000000..dbe3ca40 --- /dev/null +++ b/kortex_device_manager/srv/GetIPv4Settings.srv @@ -0,0 +1,3 @@ +Empty input +--- +IPv4Settings output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetMACAddress.srv b/kortex_device_manager/srv/GetMACAddress.srv new file mode 100644 index 00000000..8b79b417 --- /dev/null +++ b/kortex_device_manager/srv/GetMACAddress.srv @@ -0,0 +1,3 @@ +Empty input +--- +MACAddress output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetModelNumber.srv b/kortex_device_manager/srv/GetModelNumber.srv new file mode 100644 index 00000000..7d685d93 --- /dev/null +++ b/kortex_device_manager/srv/GetModelNumber.srv @@ -0,0 +1,3 @@ +Empty input +--- +ModelNumber output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetPartNumber.srv b/kortex_device_manager/srv/GetPartNumber.srv new file mode 100644 index 00000000..b4d06776 --- /dev/null +++ b/kortex_device_manager/srv/GetPartNumber.srv @@ -0,0 +1,3 @@ +Empty input +--- +PartNumber output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetPartNumberRevision.srv b/kortex_device_manager/srv/GetPartNumberRevision.srv new file mode 100644 index 00000000..dde505da --- /dev/null +++ b/kortex_device_manager/srv/GetPartNumberRevision.srv @@ -0,0 +1,3 @@ +Empty input +--- +PartNumberRevision output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetPowerOnSelfTestResult.srv b/kortex_device_manager/srv/GetPowerOnSelfTestResult.srv new file mode 100644 index 00000000..adf306ed --- /dev/null +++ b/kortex_device_manager/srv/GetPowerOnSelfTestResult.srv @@ -0,0 +1,3 @@ +Empty input +--- +PowerOnSelfTestResult output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetRunMode.srv b/kortex_device_manager/srv/GetRunMode.srv new file mode 100644 index 00000000..0b17a758 --- /dev/null +++ b/kortex_device_manager/srv/GetRunMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +RunMode output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetSafetyConfiguration.srv b/kortex_device_manager/srv/GetSafetyConfiguration.srv new file mode 100644 index 00000000..21b52ca7 --- /dev/null +++ b/kortex_device_manager/srv/GetSafetyConfiguration.srv @@ -0,0 +1,3 @@ +SafetyHandle input +--- +SafetyConfiguration output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetSafetyEnable.srv b/kortex_device_manager/srv/GetSafetyEnable.srv new file mode 100644 index 00000000..b0957315 --- /dev/null +++ b/kortex_device_manager/srv/GetSafetyEnable.srv @@ -0,0 +1,3 @@ +SafetyHandle input +--- +SafetyEnable output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetSafetyInformation.srv b/kortex_device_manager/srv/GetSafetyInformation.srv new file mode 100644 index 00000000..2869681a --- /dev/null +++ b/kortex_device_manager/srv/GetSafetyInformation.srv @@ -0,0 +1,3 @@ +SafetyHandle input +--- +SafetyInformation output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetSafetyStatus.srv b/kortex_device_manager/srv/GetSafetyStatus.srv new file mode 100644 index 00000000..c188202d --- /dev/null +++ b/kortex_device_manager/srv/GetSafetyStatus.srv @@ -0,0 +1,3 @@ +SafetyHandle input +--- +SafetyStatus output \ No newline at end of file diff --git a/kortex_device_manager/srv/GetSerialNumber.srv b/kortex_device_manager/srv/GetSerialNumber.srv new file mode 100644 index 00000000..e1bc96c2 --- /dev/null +++ b/kortex_device_manager/srv/GetSerialNumber.srv @@ -0,0 +1,3 @@ +Empty input +--- +SerialNumber output \ No newline at end of file diff --git a/kortex_device_manager/srv/ReadAllDevices.srv b/kortex_device_manager/srv/ReadAllDevices.srv new file mode 100644 index 00000000..8f01b626 --- /dev/null +++ b/kortex_device_manager/srv/ReadAllDevices.srv @@ -0,0 +1,3 @@ +Empty input +--- +DeviceHandles output \ No newline at end of file diff --git a/kortex_device_manager/srv/RebootRequest.srv b/kortex_device_manager/srv/RebootRequest.srv new file mode 100644 index 00000000..25cc1893 --- /dev/null +++ b/kortex_device_manager/srv/RebootRequest.srv @@ -0,0 +1,3 @@ +RebootRqst input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/ResetSafetyDefaults.srv b/kortex_device_manager/srv/ResetSafetyDefaults.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_device_manager/srv/ResetSafetyDefaults.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SafetyTopic.srv b/kortex_device_manager/srv/SafetyTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_device_manager/srv/SafetyTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetIPv4Settings.srv b/kortex_device_manager/srv/SetIPv4Settings.srv new file mode 100644 index 00000000..a745cb17 --- /dev/null +++ b/kortex_device_manager/srv/SetIPv4Settings.srv @@ -0,0 +1,3 @@ +IPv4Settings input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetMACAddress.srv b/kortex_device_manager/srv/SetMACAddress.srv new file mode 100644 index 00000000..33f6a894 --- /dev/null +++ b/kortex_device_manager/srv/SetMACAddress.srv @@ -0,0 +1,3 @@ +MACAddress input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetModelNumber.srv b/kortex_device_manager/srv/SetModelNumber.srv new file mode 100644 index 00000000..47eb98c4 --- /dev/null +++ b/kortex_device_manager/srv/SetModelNumber.srv @@ -0,0 +1,3 @@ +ModelNumber input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetPartNumber.srv b/kortex_device_manager/srv/SetPartNumber.srv new file mode 100644 index 00000000..12321918 --- /dev/null +++ b/kortex_device_manager/srv/SetPartNumber.srv @@ -0,0 +1,3 @@ +PartNumber input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetPartNumberRevision.srv b/kortex_device_manager/srv/SetPartNumberRevision.srv new file mode 100644 index 00000000..43c27428 --- /dev/null +++ b/kortex_device_manager/srv/SetPartNumberRevision.srv @@ -0,0 +1,3 @@ +PartNumberRevision input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetRunMode.srv b/kortex_device_manager/srv/SetRunMode.srv new file mode 100644 index 00000000..fc420a73 --- /dev/null +++ b/kortex_device_manager/srv/SetRunMode.srv @@ -0,0 +1,3 @@ +RunMode input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetSafetyConfiguration.srv b/kortex_device_manager/srv/SetSafetyConfiguration.srv new file mode 100644 index 00000000..db370ae2 --- /dev/null +++ b/kortex_device_manager/srv/SetSafetyConfiguration.srv @@ -0,0 +1,3 @@ +SafetyConfiguration input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetSafetyEnable.srv b/kortex_device_manager/srv/SetSafetyEnable.srv new file mode 100644 index 00000000..198bd121 --- /dev/null +++ b/kortex_device_manager/srv/SetSafetyEnable.srv @@ -0,0 +1,3 @@ +SafetyEnable input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetSafetyErrorThreshold.srv b/kortex_device_manager/srv/SetSafetyErrorThreshold.srv new file mode 100644 index 00000000..04c3ade4 --- /dev/null +++ b/kortex_device_manager/srv/SetSafetyErrorThreshold.srv @@ -0,0 +1,3 @@ +SafetyThreshold input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetSafetyWarningThreshold.srv b/kortex_device_manager/srv/SetSafetyWarningThreshold.srv new file mode 100644 index 00000000..04c3ade4 --- /dev/null +++ b/kortex_device_manager/srv/SetSafetyWarningThreshold.srv @@ -0,0 +1,3 @@ +SafetyThreshold input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/srv/SetSerialNumber.srv b/kortex_device_manager/srv/SetSerialNumber.srv new file mode 100644 index 00000000..f00cd0f9 --- /dev/null +++ b/kortex_device_manager/srv/SetSerialNumber.srv @@ -0,0 +1,3 @@ +SerialNumber input +--- +Empty output \ No newline at end of file diff --git a/kortex_device_manager/templates/NodeServices.cpp.jinja2 b/kortex_device_manager/templates/NodeServices.cpp.jinja2 new file mode 100644 index 00000000..52b39999 --- /dev/null +++ b/kortex_device_manager/templates/NodeServices.cpp.jinja2 @@ -0,0 +1,126 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +{% for package in detailedPackages %} +{%- if package.HasMessage == 1 -%} +#include "{{package.filename|lower}}_ros_converter.h" +#include "{{package.filename|lower}}_proto_converter.h" +{%- endif %} +{% endfor -%} + +KortexDeviceManager::KortexDeviceManager(char* ip, ros::NodeHandle& n) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + +{% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + m_{{package.name|lower}} = new {{package.name}}::{{package.name}}Client(m_router); + {%- endif -%} + +{% endfor -%} + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + std::cout << "\nSession Created\n"; + + m_pub_Error = m_n.advertise("KortexError", 1000); +{%- for package in detailedPackages -%} +{%- for method in package.service.method -%} +{%- if 'Topic' in method.name %} + m_pub_{{method.name}} = m_n.advertise("{{method.name}}", 1000); +{%- endif -%} +{%- endfor -%} +{%- endfor -%} + + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +{% for package in detailedPackages %} +{% for method in package.service.method %} +{%- if 'Topic' in method.name %} +bool KortexDeviceManager::OnNotification{{method.name}}(kortex_device_manager::{{method.name}}::Request &req, kortex_device_manager::{{method.name}}::Response &res) +{%- else %} +bool KortexDeviceManager::{{method.name}}(kortex_device_manager::{{method.name}}::Request &req, kortex_device_manager::{{method.name}}::Response &res) +{%- endif %} +{ + {%- set splitInputTypeName = method.input_type.split('.') -%} + {% set splitOutputTypeName = method.output_type.split('.') %} + {{splitInputTypeName[4]}} input; + {%- if not method.input_type.split('.')[4] == "Empty" %} + ToProtoData(req.input, &input); + {%- endif %} + {{splitOutputTypeName[4]}} output; + kortex_device_manager::KortexError result_error; + + try + { + {%- if not method.output_type.split('.')[4] == "Empty" %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + {%- if 'Topic' in method.name %} + {% set DetailedTypeName = splitInputTypeName[1:-1] %} + std::function< void ({% for word in DetailedTypeName %}{{word}}::{%- endfor -%}{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&KortexDeviceManager::cb_{{method.name}}, this, std::placeholders::_1); + output = m_{{package.name|lower}}->OnNotification{{method.name}}(callback, input); + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(input); + {%- endif %} + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(); + {%- endif %} + {%- else %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + m_{{package.name|lower}}->{{method.name}}(input); + {%- else %} + m_{{package.name|lower}}->{{method.name}}(); + {%- endif %} + {%- endif %} + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + {%- if not method.output_type.split('.')[4] == "Empty" %} + ToRosData(output, res.output); + {%- endif %} + return true; +} +{%- if 'Topic' in method.name %} +void KortexDeviceManager::cb_{{method.name}}({% for word in DetailedTypeName %}{{word}}::{%- endfor -%}{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_device_manager::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} +{% endfor -%} \ No newline at end of file diff --git a/kortex_device_manager/templates/NodeServices.h.jinja2 b/kortex_device_manager/templates/NodeServices.h.jinja2 new file mode 100644 index 00000000..3176c06b --- /dev/null +++ b/kortex_device_manager/templates/NodeServices.h.jinja2 @@ -0,0 +1,99 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{packageName}}SERVICES_H_ +#define _KORTEX_{{packageName}}SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +{%- for package in detailedPackages %} +#include <{{package.filename}}.pb.h> +{%- endfor %} + +#include +#include + +#include +#include + +{%- for package in detailedPackages %} +{%- if package.HasRPC == 1 %} +#include <{{package.name}}ClientRpc.h> +{%- endif %} +{%- endfor %} +#include +#include + +{%- for package in detailedPackages %} +{%- for method in package.service.method %} +#include "kortex_device_manager/{{method.name}}.h" +{%- endfor %} +{%- endfor %} +#include "kortex_device_manager/KortexError.h" + +using namespace std; +using namespace Kinova::Api; +{%- for package in detailedPackages %} +using namespace {{package.namespace}}; +{%- endfor %} + +class KortexDeviceManager +{ + public: + KortexDeviceManager(char* ip, ros::NodeHandle& n); +{% for package in detailedPackages %} +{%- for method in package.service.method %} +{%- if 'Topic' in method.name %} + bool OnNotification{{method.name}}(kortex_device_manager::{{method.name}}::Request &req, kortex_device_manager::{{method.name}}::Response &res); + void cb_{{method.name}}({{method.name|replace("Topic", "")}}Notification notif); +{%- else %} + bool {{method.name}}(kortex_device_manager::{{method.name}}::Request &req, kortex_device_manager::{{method.name}}::Response &res); +{%- endif %} +{%- endfor %} +{% endfor %} + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + {% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + {{package.name}}Client* m_{{package.name|lower}}; + {%- endif -%} + {% endfor %} + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + + {%- for package in detailedPackages %} + {%- for method in package.service.method %} + {%- if 'Topic' in method.name %} + ros::Publisher m_pub_{{method.name}}; + {%- endif %} + {%- endfor %} + {%- endfor %} +}; +#endif + diff --git a/kortex_device_manager/templates/main.jinja2 b/kortex_device_manager/templates/main.jinja2 new file mode 100644 index 00000000..612a3662 --- /dev/null +++ b/kortex_device_manager/templates/main.jinja2 @@ -0,0 +1,49 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "DeviceManager"); + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 1) + { + ROS_INFO("Connecting to IP = %s", argv[1]); + } + else + { + ROS_INFO("You need to provide an IP adresse as a parameter. ex: rosrun package node 192.168.1.1"); + ros::shutdown(); + return 0; + } + + KortexDeviceManager services_object(argv[1], n); + + {% for function in list_function -%} + ros::ServiceServer service{{function}} = n.advertiseService("{{function}}", &KortexDeviceManager::{{function}}, &services_object); + {% endfor %} + + ROS_INFO("Node's services initialized correctly."); + + ros::spin(); + + return 1; +} diff --git a/kortex_device_manager/templates/proto_converter.cpp.jinja2 b/kortex_device_manager/templates/proto_converter.cpp.jinja2 new file mode 100644 index 00000000..ac90acfd --- /dev/null +++ b/kortex_device_manager/templates/proto_converter.cpp.jinja2 @@ -0,0 +1,77 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentPackageName|lower}}_proto_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_proto_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToProtoData(kortex_device_manager::{{detailed_message.message.name}} input, {{detailed_message.message.name}} *output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") -%} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {%- if field.type == 11 %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + ToProtoData(input.{{field.name}}[i], output->add_{{field.name|lower}}()); + } + {%- else %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name|lower}}.size(); i++) + { + output->add_{{field.name|lower}}(input.{{field.name|lower}}[i]); + } + {% endif -%} + {% else -%} + {%- if field.type == 11 %} + ToProtoData(input.{{field.name}}, output->mutable_{{field.name}}()); + {%- elif field.type == 14 %}{# ENUM #} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output->set_{{field.name}}(({{field.type_name|replace(".", "", 1)|replace(".", "::")}})input.{{field.name}}); + {%- elif field.type == 12 %} + output->set_{{field.name}}(std::string(input.{{field.name}}.begin(), input.{{field.name}}.end())); + {%- else %} + output->set_{{field.name}}(input.{{field.name}}); + {%- endif -%} + {%- endif -%} + {%- endif -%} + {%- endfor -%} + + {% if detailed_message.HasOneOf == "true" %} + + {% for field in detailed_message.message.field %} + {%- if field.HasField("oneof_index") -%} + if(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.size() > 0) + { + {% if field.type == 11 -%} + ToProtoData(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0], output->mutable_{{field.name}}()); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} + output->set_{{field.name}}(({{list1[4]}})input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- else %} + output->set_{{field.name}}(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- endif %} + } + {% endif %} + {%- endfor -%} + {% endif %} + + return 0; +} +{% endfor %} diff --git a/kortex_device_manager/templates/proto_converter.h.jinja2 b/kortex_device_manager/templates/proto_converter.h.jinja2 new file mode 100644 index 00000000..b9b0c1c6 --- /dev/null +++ b/kortex_device_manager/templates/proto_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_device_manager/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToProtoData(kortex_device_manager::{{detailed_message.message.name}} intput, {{detailed_message.message.name}} *output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_device_manager/templates/ros_converter.cpp.jinja2 b/kortex_device_manager/templates/ros_converter.cpp.jinja2 new file mode 100644 index 00000000..8eb6f01e --- /dev/null +++ b/kortex_device_manager/templates/ros_converter.cpp.jinja2 @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentPackageName|lower}}_ros_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_ros_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_device_manager::{{detailed_message.message.name}} &output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") %} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {% if field.type == 11 %} + {%- set splitTypeName = field.type_name.split('.') -%} + output.{{field.name|lower}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + kortex_device_manager::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(i), temp); + output.{{field.name}}.push_back(temp); + } + {%- else %} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + output.{{field.name}}.push_back(input.{{field.name|lower}}(i)); + } + {%- endif %} + {%- else %} + {%- if field.type == 11 %} + ToRosData(input.{{field.name}}(), output.{{field.name}}); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output.{{field.name}} = input.{{field.name}}(); + {%- elif field.type == 12 %} + output.{{field.name}} = std::vector(input.{{field.name}}().begin(), input.{{field.name}}().end()); + {%- else %} + output.{{field.name}} = input.{{field.name}}(); + {%- endif %} + {%- endif %} + {%- endif %} + {%- endfor %} + + {% if detailed_message.HasOneOf == "true" %} + auto oneof_type = input.{{detailed_message.message.ListFields()[-1][1][0].name}}_case(); + + switch(oneof_type) + { + {%- for field in detailed_message.message.field -%} + {%- if field.HasField("oneof_index") -%} + {%- set splitTypeName = field.type_name.split('.') %} + {%- set EnumName = field.name.replace("_", " ").title().replace(" ", "") %} + case {{detailed_message.message.name}}::k{{EnumName}}: + { + {%- if field.type == 11 %} + kortex_device_manager::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(), temp); + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(temp); + {%- elif field.type == 14 %} + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(input.{{field.name}}()); + {% endif %} + break; + } + {% endif %} + {%- endfor %} + } + {% endif -%} + + return 0; +} +{% endfor %} diff --git a/kortex_device_manager/templates/ros_converter.h.jinja2 b/kortex_device_manager/templates/ros_converter.h.jinja2 new file mode 100644 index 00000000..5f78c4d1 --- /dev/null +++ b/kortex_device_manager/templates/ros_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_device_manager/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_device_manager::{{detailed_message.message.name}} &output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_device_manager/templates/ros_enum.jinja2 b/kortex_device_manager/templates/ros_enum.jinja2 new file mode 100644 index 00000000..164146fc --- /dev/null +++ b/kortex_device_manager/templates/ros_enum.jinja2 @@ -0,0 +1,3 @@ +{% for member in item.value %} +uint32 {{member.name}} = {{member.number}} +{% endfor %} diff --git a/kortex_device_manager/templates/ros_message.jinja2 b/kortex_device_manager/templates/ros_message.jinja2 new file mode 100644 index 00000000..746188b6 --- /dev/null +++ b/kortex_device_manager/templates/ros_message.jinja2 @@ -0,0 +1,44 @@ +{%- for member in item.field -%} +{%- if not member.HasField("oneof_index") -%} +{%- if member.type == 9 %} {# TYPE_STRING #} +string{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 12 %} {# TYPE_BYTES #} +uint8[] {{member.name}} +{%- elif member.type == 1 %} {# TYPE_DOUBLE #} +float64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 7 %} {# TYPE_FIXED32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 6 %} {# TYPE_FIXED64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 2 %} {# TYPE_FLOAT #} +float32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 5 %} {# TYPE_INT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 3 %} {# TYPE_INT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 15 %} {# TYPE_SFIXED32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 16 %} {# TYPE_SFIXED64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 17 %} {# TYPE_SINT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 18 %} {# TYPE_SINT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 13 %} {# TYPE_UINT32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 4 %} {# TYPE_UINT64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 14 -%} {# TYPE_ENUM #} +{% set list1 = member.type_name.split('.') %} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 8 %} {# TYPE_BOOL #} +bool {{member.name}} +{%- elif member.type == 11 %}{# TYPE MESSAGE #} +{% set list1 = member.type_name.split('.') %} +{{list1[4]}}{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- endif -%} +{%- endif -%} +{%- endfor -%} +{%- if HasOneOf %} +{{item.name}}_{{item.ListFields()[-1][1][0].name}} oneof_{{item.ListFields()[-1][1][0].name}} +{%- endif -%} \ No newline at end of file diff --git a/kortex_device_manager/templates/ros_oneof.jinja2 b/kortex_device_manager/templates/ros_oneof.jinja2 new file mode 100644 index 00000000..4fac302a --- /dev/null +++ b/kortex_device_manager/templates/ros_oneof.jinja2 @@ -0,0 +1,9 @@ +{%- for member in item.field -%} +{% if member.HasField("oneof_index") %} +{% if member.type == 11 %} +{% set list1 = member.type_name.split('.') %}{{list1[4]}}[] {{member.name}} +{%- else -%} +uint32[] {{member.name}} +{%- endif -%} +{%- endif -%} +{% endfor %} \ No newline at end of file diff --git a/kortex_device_manager/templates/ros_service.jinja2 b/kortex_device_manager/templates/ros_service.jinja2 new file mode 100644 index 00000000..cc015cf8 --- /dev/null +++ b/kortex_device_manager/templates/ros_service.jinja2 @@ -0,0 +1,5 @@ +{% set split_input_type = item.input_type.split('.') %} +{%- set split_output_type = item.output_type.split('.') -%} +{{split_input_type[4]}} input +--- +{{split_output_type[4]}} output \ No newline at end of file diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt new file mode 100644 index 00000000..c0d83b16 --- /dev/null +++ b/kortex_driver/CMakeLists.txt @@ -0,0 +1,39 @@ + +cmake_minimum_required(VERSION 2.8.3) +project(kortex_driver) + +add_compile_options(-std=c++11) +add_definitions(-D_OS_UNIX) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) + +file(GLOB_RECURSE cpp_list RELATIVE ${PROJECT_SOURCE_DIR} "src/*.cpp") + +## Declare ROS messages and services +add_message_files(DIRECTORY msg) +add_message_files(DIRECTORY msg/non_generated) +add_service_files(DIRECTORY srv) +add_service_files(DIRECTORY srv/non_generated) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${PROJECT_SOURCE_DIR}/src) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client_stubs) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/messages) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/common) +include_directories(include ${PROJECT_SOURCE_DIR}/src/util) + +link_directories(${PROJECT_SOURCE_DIR}/../kortex_api/lib/release) + +add_executable(${PROJECT_NAME} ${cpp_list}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} CppKinovaApi gcov) + +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) diff --git a/kortex_driver/RosGeneration.py b/kortex_driver/RosGeneration.py new file mode 100644 index 00000000..27a56cf7 --- /dev/null +++ b/kortex_driver/RosGeneration.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python + +import sys + +from google.protobuf.compiler import plugin_pb2 as plugin +from google.protobuf import json_format as json_f + +import jinja2 + +import itertools +import json +import types +import os +import sys + +from google.protobuf.descriptor_pb2 import DescriptorProto, EnumDescriptorProto, ServiceDescriptorProto, FieldDescriptorProto, OneofDescriptorProto + +#Class that holds a protobuf message and some other details needed by the generator(jinja2 template). +class DetailedMessage: + def __init__(self, message=None): + self.message = message + self.HasOneOf = "false" + self.oneOfList = [] + +#Class that holds a protobuf service and some other details needed by the generator(jinja2 template). +class DetailedPackage: + def __init__(self, service=None): + self.name = "NoName" + self.service = service + +#JINJA2 function to render a file from a template. +def render(tpl_path, context): + path, filename = os.path.split(tpl_path) + return jinja2.Environment(loader=jinja2.FileSystemLoader(path or './')).get_template(filename).render(**context) + +#Main plugin function +def generate_code(request, response): + + #The context is the object sent to the JINJA2 template + context = types.SimpleNamespace() + context.serviceVersion = 1 + + context.detailedPackages = [] + + MainFilePath = os.path.join(".", "src/main.cpp") + function_list = [] + fileIndex = 0 + + for proto_file in request.proto_file: + context.detailedPackages.append(DetailedPackage()) + context.detailedPackages[fileIndex].name = proto_file.package.split(".")[-1] + context.detailedPackages[fileIndex].filename = proto_file.name.split(".")[0] + context.detailedPackages[fileIndex].namespace = proto_file.package.replace(".", "::") + context.detailedPackages[fileIndex].HasRPC = 0 + context.detailedPackages[fileIndex].HasMessage = 0 + + HeaderFilePath = os.path.join(".", "src/node.h") + CppFilePath = os.path.join(".", "src/node.cpp") + + package_name = str(proto_file.package) + + service_name = proto_file.package.split('.')[-1] + '/' + + #We lower the case to respect ROS coding standard style + CppProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.h".format(proto_file.name.split(".")[0].lower())) + CppRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.h".format(proto_file.name.split(".")[0].lower())) + + list_detailedMessage = [] + list_detailedMethod = [] + + # For every item in the current proto file + for item, package in traverse(proto_file): + context.HasOneOf = 0 + + if isinstance(item, EnumDescriptorProto): + context.item = item + + ros_enumPath = os.path.join(".", "msg/{}.msg".format(item.name)) + + with open(ros_enumPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_enum.jinja2", context.__dict__)) + #If this it a message + if isinstance(item, DescriptorProto): + tempMessage = DetailedMessage(item) + context.detailedPackages[fileIndex].HasMessage = 1 + + for member in item.field: + #If a member is part of a oneof, it will have this additional field + if member.HasField("oneof_index"): + context.HasOneOf = 1 + tempMessage.HasOneOf = "true" + else: + context.HasOneOf = 0 + tempMessage.HasOneOf = "false" + + context.item = item + + #If the proto file contains a ONEOF we need to generate a separate file to handle it. + if context.HasOneOf == 1: + + #This line gets the list of ONEOF that is in the current message. + oneOfList = item.ListFields()[-1][1] + + tempMessage.oneOfList = item.ListFields()[-1][1] + ros_oneofPath = os.path.join(".", "msg/{}_{}.msg".format(item.name, oneOfList[0].name)) + + with open(ros_oneofPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_oneof.jinja2", context.__dict__)) + + + list_detailedMessage.append(tempMessage) + ros_messagePath = os.path.join(".", "msg/{}.msg".format(item.name)) + + #We call jinja2 to generate a ROS message. + with open(ros_messagePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_message.jinja2", context.__dict__)) + + #If this is a service (A group of method) + if isinstance(item, ServiceDescriptorProto): + for idx, method in enumerate(item.method): + context.item = method + if "Topic" not in method.name: + function_list.append(method.name) + ros_servicePath = os.path.join(".", "srv/{}.srv".format(method.name)) + with open(ros_servicePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_service.jinja2", context.__dict__)) + + context.detailedPackages[fileIndex].service = item + context.detailedPackages[fileIndex].HasRPC = 1 + + context.currentPackageName = context.detailedPackages[fileIndex].name + context.currentNamespace = proto_file.package.replace(".", "::") + context.currentFilename = context.detailedPackages[fileIndex].filename + context.item = list_detailedMessage + + if context.detailedPackages[fileIndex].HasMessage == 1: + #Wecall jinja2 to generate a prot/ROS converter for every protobuf message. + with open(CppProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.cpp.jinja2", context.__dict__)) + with open(HeaderProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.h.jinja2", context.__dict__)) + with open(CppRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.cpp.jinja2", context.__dict__)) + with open(HeaderRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.h.jinja2", context.__dict__)) + + fileIndex = fileIndex + 1 + + context.list_function = function_list + + #We jinja2 to generate the ROS node. + with open(HeaderFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.h.jinja2", context.__dict__)) + with open(CppFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.cpp.jinja2", context.__dict__)) + with open(MainFilePath, 'wt') as mainFile: + mainFile.write(render("./templates/main.jinja2", context.__dict__)) + +def traverse(proto_file): + #recursive function that browse a protobof item + def _traverse(package, items): + for item in items: + yield item, package + + if isinstance(item, DescriptorProto): + for enum in item.enum_type: + yield enum, package + + for nested in item.nested_type: + nested_package = package + item.name + + for nested_item in _traverse(nested, nested_package): + yield nested_item, nested_package + if isinstance(item, ServiceDescriptorProto): + for rpc in item.method: + yield rpc, package + + #return a list of everything found in the proto file + return itertools.chain( + _traverse(proto_file.package, proto_file.enum_type), + _traverse(proto_file.package, proto_file.message_type), + _traverse(proto_file.package, proto_file.service), + ) + +if __name__ == '__main__': + # Read request message from stdin + data = sys.stdin.buffer.read() + + # Parse request + request = plugin.CodeGeneratorRequest() + request.ParseFromString(data) + + # Create response + response = plugin.CodeGeneratorResponse() + + # Generate code + generate_code(request, response) + + # Serialise response message + output = response.SerializeToString() + + # Write to stdout + sys.stdout.buffer.write(output) \ No newline at end of file diff --git a/kortex_driver/build/.gitignore b/kortex_driver/build/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_driver/kortex_driver.sh b/kortex_driver/kortex_driver.sh new file mode 100755 index 00000000..011eee96 --- /dev/null +++ b/kortex_driver/kortex_driver.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +python3 -u RosGeneration.py + diff --git a/kortex_driver/msg/Action.msg b/kortex_driver/msg/Action.msg new file mode 100644 index 00000000..3ef9cdef --- /dev/null +++ b/kortex_driver/msg/Action.msg @@ -0,0 +1,6 @@ + + +ActionHandle handle +string name +string application_data +Action_action_parameters oneof_action_parameters \ No newline at end of file diff --git a/kortex_driver/msg/ActionEvent.msg b/kortex_driver/msg/ActionEvent.msg new file mode 100644 index 00000000..b06938f2 --- /dev/null +++ b/kortex_driver/msg/ActionEvent.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_ACTION_EVENT = 0 + +uint32 ACTION_END = 1 + +uint32 ACTION_ABORT = 2 + +uint32 ACTION_PAUSE = 3 + +uint32 ACTION_START = 4 diff --git a/kortex_driver/msg/ActionExecutionState.msg b/kortex_driver/msg/ActionExecutionState.msg new file mode 100644 index 00000000..8737fa01 --- /dev/null +++ b/kortex_driver/msg/ActionExecutionState.msg @@ -0,0 +1,5 @@ + + +uint32 action_event + +ActionHandle handle \ No newline at end of file diff --git a/kortex_driver/msg/ActionHandle.msg b/kortex_driver/msg/ActionHandle.msg new file mode 100644 index 00000000..8cf5911e --- /dev/null +++ b/kortex_driver/msg/ActionHandle.msg @@ -0,0 +1,5 @@ + +uint32 identifier + +uint32 action_type +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/ActionList.msg b/kortex_driver/msg/ActionList.msg new file mode 100644 index 00000000..e8923a65 --- /dev/null +++ b/kortex_driver/msg/ActionList.msg @@ -0,0 +1,3 @@ + + +Action[] action_list \ No newline at end of file diff --git a/kortex_driver/msg/ActionNotification.msg b/kortex_driver/msg/ActionNotification.msg new file mode 100644 index 00000000..934e4528 --- /dev/null +++ b/kortex_driver/msg/ActionNotification.msg @@ -0,0 +1,13 @@ + + +uint32 action_event + +ActionHandle handle + +Timestamp timestamp + +UserProfileHandle user_handle + +uint32 abort_details + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ActionNotificationList.msg b/kortex_driver/msg/ActionNotificationList.msg new file mode 100644 index 00000000..cd2959a4 --- /dev/null +++ b/kortex_driver/msg/ActionNotificationList.msg @@ -0,0 +1,3 @@ + + +ActionNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/ActionType.msg b/kortex_driver/msg/ActionType.msg new file mode 100644 index 00000000..a9159a07 --- /dev/null +++ b/kortex_driver/msg/ActionType.msg @@ -0,0 +1,34 @@ + +uint32 UNSPECIFIED_ACTION = 0 + +uint32 SEND_TWIST_COMMAND = 1 + +uint32 SEND_JOINT_SPEEDS = 4 + +uint32 REACH_POSE = 6 + +uint32 REACH_JOINT_ANGLES = 7 + +uint32 TOGGLE_ADMITTANCE_MODE = 13 + +uint32 SWITCH_CONTROL_MAPPING = 16 + +uint32 NAVIGATE_JOINTS = 17 + +uint32 NAVIGATE_MAPPINGS = 18 + +uint32 CHANGE_TWIST = 22 + +uint32 CHANGE_JOINT_SPEEDS = 23 + +uint32 APPLY_EMERGENCY_STOP = 28 + +uint32 CLEAR_FAULTS = 29 + +uint32 DELAY = 31 + +uint32 EXECUTE_ACTION = 32 + +uint32 SEND_GRIPPER_COMMAND = 33 + +uint32 STOP_ACTION = 35 diff --git a/kortex_driver/msg/Action_action_parameters.msg b/kortex_driver/msg/Action_action_parameters.msg new file mode 100644 index 00000000..63edadef --- /dev/null +++ b/kortex_driver/msg/Action_action_parameters.msg @@ -0,0 +1,30 @@ + + +TwistCommand[] send_twist_command + +JointSpeeds[] send_joint_speeds + +ConstrainedPose[] reach_pose + +ConstrainedJointAngles[] reach_joint_angles +uint32[] toggle_admittance_mode + +SwitchControlMapping[] switch_control_mapping +uint32[] navigate_joints +uint32[] navigate_mappings + +ChangeTwist[] change_twist + +ChangeJointSpeeds[] change_joint_speeds + +EmergencyStop[] apply_emergency_stop + +Faults[] clear_faults + +Delay[] delay + +ActionHandle[] execute_action + +GripperCommand[] send_gripper_command + +Stop[] stop_action \ No newline at end of file diff --git a/kortex_driver/msg/ActivateMapHandle.msg b/kortex_driver/msg/ActivateMapHandle.msg new file mode 100644 index 00000000..184513e9 --- /dev/null +++ b/kortex_driver/msg/ActivateMapHandle.msg @@ -0,0 +1,7 @@ + + +MappingHandle mapping_handle + +MapGroupHandle map_group_handle + +MapHandle map_handle \ No newline at end of file diff --git a/kortex_driver/msg/ActuatorCommand.msg b/kortex_driver/msg/ActuatorCommand.msg new file mode 100644 index 00000000..a3d39bb4 --- /dev/null +++ b/kortex_driver/msg/ActuatorCommand.msg @@ -0,0 +1,7 @@ + +uint32 command_id +uint32 flags +float32 position +float32 velocity +float32 torque_joint +float32 current_motor \ No newline at end of file diff --git a/kortex_driver/msg/ActuatorCustomData.msg b/kortex_driver/msg/ActuatorCustomData.msg new file mode 100644 index 00000000..1262a849 --- /dev/null +++ b/kortex_driver/msg/ActuatorCustomData.msg @@ -0,0 +1,18 @@ + +uint32 command_id +uint32 custom_data_0 +uint32 custom_data_1 +uint32 custom_data_2 +uint32 custom_data_3 +uint32 custom_data_4 +uint32 custom_data_5 +uint32 custom_data_6 +uint32 custom_data_7 +uint32 custom_data_8 +uint32 custom_data_9 +uint32 custom_data_10 +uint32 custom_data_11 +uint32 custom_data_12 +uint32 custom_data_13 +uint32 custom_data_14 +uint32 custom_data_15 \ No newline at end of file diff --git a/kortex_driver/msg/ActuatorFeedback.msg b/kortex_driver/msg/ActuatorFeedback.msg new file mode 100644 index 00000000..0ce2e22c --- /dev/null +++ b/kortex_driver/msg/ActuatorFeedback.msg @@ -0,0 +1,15 @@ + +uint32 command_id +uint32 status_flags +uint32 jitter_comm +float32 position +float32 velocity +float32 torque +float32 current_motor +float32 voltage +float32 temperature_motor +float32 temperature_core +uint32 fault_bank_a +uint32 fault_bank_b +uint32 warning_bank_a +uint32 warning_bank_b \ No newline at end of file diff --git a/kortex_driver/msg/ActuatorInformation.msg b/kortex_driver/msg/ActuatorInformation.msg new file mode 100644 index 00000000..33727e06 --- /dev/null +++ b/kortex_driver/msg/ActuatorInformation.msg @@ -0,0 +1,2 @@ + +uint32 count \ No newline at end of file diff --git a/kortex_driver/msg/Admittance.msg b/kortex_driver/msg/Admittance.msg new file mode 100644 index 00000000..619ce458 --- /dev/null +++ b/kortex_driver/msg/Admittance.msg @@ -0,0 +1,3 @@ + + +uint32 admittance_mode \ No newline at end of file diff --git a/kortex_driver/msg/AdmittanceMode.msg b/kortex_driver/msg/AdmittanceMode.msg new file mode 100644 index 00000000..95037f23 --- /dev/null +++ b/kortex_driver/msg/AdmittanceMode.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_ADMITTANCE_MODE = 0 + +uint32 CARTESIAN = 1 + +uint32 JOINT = 2 + +uint32 NULL_SPACE = 3 + +uint32 DISABLED = 4 diff --git a/kortex_driver/msg/AdvancedSequenceHandle.msg b/kortex_driver/msg/AdvancedSequenceHandle.msg new file mode 100644 index 00000000..0a2bed61 --- /dev/null +++ b/kortex_driver/msg/AdvancedSequenceHandle.msg @@ -0,0 +1,4 @@ + + +SequenceHandle handle +bool in_loop \ No newline at end of file diff --git a/kortex_driver/msg/AppendActionInformation.msg b/kortex_driver/msg/AppendActionInformation.msg new file mode 100644 index 00000000..6e47fe18 --- /dev/null +++ b/kortex_driver/msg/AppendActionInformation.msg @@ -0,0 +1,5 @@ + + +SequenceHandle sequence_handle + +Action action \ No newline at end of file diff --git a/kortex_driver/msg/ArmState.msg b/kortex_driver/msg/ArmState.msg new file mode 100644 index 00000000..d85bd69d --- /dev/null +++ b/kortex_driver/msg/ArmState.msg @@ -0,0 +1,22 @@ + +uint32 UNSPECIFIED_ARM_STATE = 0 + +uint32 BASE_INITIALIZATION = 1 + +uint32 IDLE = 2 + +uint32 ARM_INITIALIZATION = 3 + +uint32 ARM_IN_FAULT = 4 + +uint32 ARM_MAINTENANCE = 5 + +uint32 ARM_SERVOING_LOW_LEVEL = 6 + +uint32 ARM_SERVOING_READY = 7 + +uint32 ARM_SERVOING_PLAYING_SEQUENCE = 8 + +uint32 ARM_SERVOING_MANUALLY_CONTROLLED = 9 + +uint32 RESERVED = 255 diff --git a/kortex_driver/msg/ArmStateInformation.msg b/kortex_driver/msg/ArmStateInformation.msg new file mode 100644 index 00000000..31839b05 --- /dev/null +++ b/kortex_driver/msg/ArmStateInformation.msg @@ -0,0 +1,5 @@ + + +uint32 active_state + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ArmStateNotification.msg b/kortex_driver/msg/ArmStateNotification.msg new file mode 100644 index 00000000..b5e1436f --- /dev/null +++ b/kortex_driver/msg/ArmStateNotification.msg @@ -0,0 +1,7 @@ + + +uint32 active_state + +Timestamp timestamp + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/BackupEvent.msg b/kortex_driver/msg/BackupEvent.msg new file mode 100644 index 00000000..28f21b35 --- /dev/null +++ b/kortex_driver/msg/BackupEvent.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_BACKUP_EVENT = 0 + +uint32 BACKUP_RESTORED = 1 + +uint32 BACKUP_UPLOADED = 2 diff --git a/kortex_driver/msg/BaseFeedback.msg b/kortex_driver/msg/BaseFeedback.msg new file mode 100644 index 00000000..10c3b325 --- /dev/null +++ b/kortex_driver/msg/BaseFeedback.msg @@ -0,0 +1,36 @@ + +uint32 active_state_connection_identifier + +uint32 active_state +float32 arm_voltage +float32 arm_current +float32 temperature_cpu +float32 temperature_ambient +float32 imu_acceleration_x +float32 imu_acceleration_y +float32 imu_acceleration_z +float32 imu_angular_velocity_x +float32 imu_angular_velocity_y +float32 imu_angular_velocity_z +float32 tool_pose_x +float32 tool_pose_y +float32 tool_pose_z +float32 tool_pose_theta_x +float32 tool_pose_theta_y +float32 tool_pose_theta_z +float32 tool_twist_linear_x +float32 tool_twist_linear_y +float32 tool_twist_linear_z +float32 tool_twist_angular_x +float32 tool_twist_angular_y +float32 tool_twist_angular_z +float32 tool_external_wrench_force_x +float32 tool_external_wrench_force_y +float32 tool_external_wrench_force_z +float32 tool_external_wrench_torque_x +float32 tool_external_wrench_torque_y +float32 tool_external_wrench_torque_z +uint32 fault_bank_a +uint32 fault_bank_b +uint32 warning_bank_a +uint32 warning_bank_b \ No newline at end of file diff --git a/kortex_driver/msg/CartesianLimitation.msg b/kortex_driver/msg/CartesianLimitation.msg new file mode 100644 index 00000000..d47b4fd5 --- /dev/null +++ b/kortex_driver/msg/CartesianLimitation.msg @@ -0,0 +1,5 @@ + + +uint32 type +float32 translation +float32 orientation \ No newline at end of file diff --git a/kortex_driver/msg/CartesianLimitationList.msg b/kortex_driver/msg/CartesianLimitationList.msg new file mode 100644 index 00000000..5ae6e2ab --- /dev/null +++ b/kortex_driver/msg/CartesianLimitationList.msg @@ -0,0 +1,3 @@ + + +CartesianLimitation[] limitations \ No newline at end of file diff --git a/kortex_driver/msg/CartesianReferenceFrame.msg b/kortex_driver/msg/CartesianReferenceFrame.msg new file mode 100644 index 00000000..b25dfb05 --- /dev/null +++ b/kortex_driver/msg/CartesianReferenceFrame.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_CARTESIAN_REFERENCE_FRAME = 0 + +uint32 MIXED = 1 + +uint32 TOOL = 2 diff --git a/kortex_driver/msg/CartesianReferenceFrameRequest.msg b/kortex_driver/msg/CartesianReferenceFrameRequest.msg new file mode 100644 index 00000000..9b0d748b --- /dev/null +++ b/kortex_driver/msg/CartesianReferenceFrameRequest.msg @@ -0,0 +1,3 @@ + + +uint32 reference_frame \ No newline at end of file diff --git a/kortex_driver/msg/CartesianSpeed.msg b/kortex_driver/msg/CartesianSpeed.msg new file mode 100644 index 00000000..7f4d31ee --- /dev/null +++ b/kortex_driver/msg/CartesianSpeed.msg @@ -0,0 +1,3 @@ + +float32 translation +float32 orientation \ No newline at end of file diff --git a/kortex_driver/msg/CartesianTrajectoryConstraint.msg b/kortex_driver/msg/CartesianTrajectoryConstraint.msg new file mode 100644 index 00000000..eb04b73e --- /dev/null +++ b/kortex_driver/msg/CartesianTrajectoryConstraint.msg @@ -0,0 +1,2 @@ + +CartesianTrajectoryConstraint_type oneof_type \ No newline at end of file diff --git a/kortex_driver/msg/CartesianTrajectoryConstraint_type.msg b/kortex_driver/msg/CartesianTrajectoryConstraint_type.msg new file mode 100644 index 00000000..5346af8c --- /dev/null +++ b/kortex_driver/msg/CartesianTrajectoryConstraint_type.msg @@ -0,0 +1,4 @@ + + +CartesianSpeed[] speed +uint32[] duration \ No newline at end of file diff --git a/kortex_driver/msg/ChangeJointSpeeds.msg b/kortex_driver/msg/ChangeJointSpeeds.msg new file mode 100644 index 00000000..c6f65586 --- /dev/null +++ b/kortex_driver/msg/ChangeJointSpeeds.msg @@ -0,0 +1,3 @@ + + +JointSpeeds joint_speeds \ No newline at end of file diff --git a/kortex_driver/msg/ChangeTwist.msg b/kortex_driver/msg/ChangeTwist.msg new file mode 100644 index 00000000..0c8b6d2d --- /dev/null +++ b/kortex_driver/msg/ChangeTwist.msg @@ -0,0 +1,3 @@ + +float32 linear +float32 angular \ No newline at end of file diff --git a/kortex_driver/msg/Command.msg b/kortex_driver/msg/Command.msg new file mode 100644 index 00000000..0e61a1d5 --- /dev/null +++ b/kortex_driver/msg/Command.msg @@ -0,0 +1,6 @@ + +uint32 frame_id + +ActuatorCommand[] actuators + +InterconnectCommand interconnect \ No newline at end of file diff --git a/kortex_driver/msg/CommunicationInterfaceConfiguration.msg b/kortex_driver/msg/CommunicationInterfaceConfiguration.msg new file mode 100644 index 00000000..9ff04314 --- /dev/null +++ b/kortex_driver/msg/CommunicationInterfaceConfiguration.msg @@ -0,0 +1,4 @@ + + +uint32 type +bool enable \ No newline at end of file diff --git a/kortex_driver/msg/ConfigurationChangeNotification.msg b/kortex_driver/msg/ConfigurationChangeNotification.msg new file mode 100644 index 00000000..40a0c973 --- /dev/null +++ b/kortex_driver/msg/ConfigurationChangeNotification.msg @@ -0,0 +1,9 @@ + + +uint32 event + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ConfigurationChangeNotificationList.msg b/kortex_driver/msg/ConfigurationChangeNotificationList.msg new file mode 100644 index 00000000..120fcbbc --- /dev/null +++ b/kortex_driver/msg/ConfigurationChangeNotificationList.msg @@ -0,0 +1,3 @@ + + +ConfigurationChangeNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/ConfigurationNotificationEvent.msg b/kortex_driver/msg/ConfigurationNotificationEvent.msg new file mode 100644 index 00000000..6614d7ea --- /dev/null +++ b/kortex_driver/msg/ConfigurationNotificationEvent.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_CONFIGURATION_EVENT = 0 + +uint32 UPDATE = 1 + +uint32 DELETE = 2 + +uint32 DELETE_ALL = 3 + +uint32 CREATE = 4 diff --git a/kortex_driver/msg/Connection.msg b/kortex_driver/msg/Connection.msg new file mode 100644 index 00000000..ca08b6c8 --- /dev/null +++ b/kortex_driver/msg/Connection.msg @@ -0,0 +1,5 @@ + + +UserProfileHandle user_handle +string connection_information +uint32 connection_identifier \ No newline at end of file diff --git a/kortex_driver/msg/ConstrainedJointAngle.msg b/kortex_driver/msg/ConstrainedJointAngle.msg new file mode 100644 index 00000000..25394c23 --- /dev/null +++ b/kortex_driver/msg/ConstrainedJointAngle.msg @@ -0,0 +1,5 @@ + +uint32 joint_identifier +float32 value + +JointTrajectoryConstraint constraint \ No newline at end of file diff --git a/kortex_driver/msg/ConstrainedJointAngles.msg b/kortex_driver/msg/ConstrainedJointAngles.msg new file mode 100644 index 00000000..2d1c3a17 --- /dev/null +++ b/kortex_driver/msg/ConstrainedJointAngles.msg @@ -0,0 +1,5 @@ + + +JointAngles joint_angles + +JointTrajectoryConstraint constraint \ No newline at end of file diff --git a/kortex_driver/msg/ConstrainedOrientation.msg b/kortex_driver/msg/ConstrainedOrientation.msg new file mode 100644 index 00000000..3d45805e --- /dev/null +++ b/kortex_driver/msg/ConstrainedOrientation.msg @@ -0,0 +1,5 @@ + + +Orientation target_orientation + +CartesianTrajectoryConstraint constraint \ No newline at end of file diff --git a/kortex_driver/msg/ConstrainedPose.msg b/kortex_driver/msg/ConstrainedPose.msg new file mode 100644 index 00000000..4dc894f8 --- /dev/null +++ b/kortex_driver/msg/ConstrainedPose.msg @@ -0,0 +1,5 @@ + + +Pose target_pose + +CartesianTrajectoryConstraint constraint \ No newline at end of file diff --git a/kortex_driver/msg/ConstrainedPosition.msg b/kortex_driver/msg/ConstrainedPosition.msg new file mode 100644 index 00000000..64dec6fe --- /dev/null +++ b/kortex_driver/msg/ConstrainedPosition.msg @@ -0,0 +1,5 @@ + + +Position target_position + +CartesianTrajectoryConstraint constraint \ No newline at end of file diff --git a/kortex_driver/msg/ControlMode.msg b/kortex_driver/msg/ControlMode.msg new file mode 100644 index 00000000..7e5d154c --- /dev/null +++ b/kortex_driver/msg/ControlMode.msg @@ -0,0 +1,20 @@ + +uint32 UNSPECIFIED_CONTROL_MODE = 0 + +uint32 ANGULAR_JOYSTICK = 1 + +uint32 CARTESIAN_JOYSTICK = 2 + +uint32 VISION_JOYSTICK = 3 + +uint32 ANGULAR_TRAJECTORY = 4 + +uint32 CARTESIAN_TRAJECTORY = 5 + +uint32 CARTESIAN_ADMITTANCE = 6 + +uint32 JOINT_ADMITTANCE = 7 + +uint32 NULL_SPACE_ADMITTANCE = 8 + +uint32 IDLE = 13 diff --git a/kortex_driver/msg/ControlModeInformation.msg b/kortex_driver/msg/ControlModeInformation.msg new file mode 100644 index 00000000..bdde570c --- /dev/null +++ b/kortex_driver/msg/ControlModeInformation.msg @@ -0,0 +1,3 @@ + + +uint32 mode \ No newline at end of file diff --git a/kortex_driver/msg/ControlModeNotification.msg b/kortex_driver/msg/ControlModeNotification.msg new file mode 100644 index 00000000..aa8fae5b --- /dev/null +++ b/kortex_driver/msg/ControlModeNotification.msg @@ -0,0 +1,9 @@ + + +uint32 control_mode + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ControlModeNotificationList.msg b/kortex_driver/msg/ControlModeNotificationList.msg new file mode 100644 index 00000000..14938d84 --- /dev/null +++ b/kortex_driver/msg/ControlModeNotificationList.msg @@ -0,0 +1,3 @@ + + +ControlModeNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/ControllerBehavior.msg b/kortex_driver/msg/ControllerBehavior.msg new file mode 100644 index 00000000..cebfc841 --- /dev/null +++ b/kortex_driver/msg/ControllerBehavior.msg @@ -0,0 +1,12 @@ + +uint32 UNSPECIFIED_CONTROLLER_BEHAVIOR = 0 + +uint32 CONTROLLER_BUTTON_DOWN = 1 + +uint32 CONTROLLER_BUTTON_UP = 2 + +uint32 CONTROLLER_AXIS_POSITIVE = 3 + +uint32 CONTROLLER_AXIS_NEGATIVE = 4 + +uint32 CONTROLLER_BUTTON_CLICK = 5 diff --git a/kortex_driver/msg/ControllerElementEventType.msg b/kortex_driver/msg/ControllerElementEventType.msg new file mode 100644 index 00000000..47c3b0b3 --- /dev/null +++ b/kortex_driver/msg/ControllerElementEventType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_CONTROLLER_ELEMENT_EVENT = 0 + +uint32 AXIS_MOVED = 1 + +uint32 BUTTON_DOWN = 2 + +uint32 BUTTON_UP = 3 diff --git a/kortex_driver/msg/ControllerElementHandle.msg b/kortex_driver/msg/ControllerElementHandle.msg new file mode 100644 index 00000000..61946e5f --- /dev/null +++ b/kortex_driver/msg/ControllerElementHandle.msg @@ -0,0 +1,4 @@ + + +ControllerHandle controller_handle +ControllerElementHandle_identifier oneof_identifier \ No newline at end of file diff --git a/kortex_driver/msg/ControllerElementHandle_identifier.msg b/kortex_driver/msg/ControllerElementHandle_identifier.msg new file mode 100644 index 00000000..142ae973 --- /dev/null +++ b/kortex_driver/msg/ControllerElementHandle_identifier.msg @@ -0,0 +1,3 @@ + +uint32[] button +uint32[] axis \ No newline at end of file diff --git a/kortex_driver/msg/ControllerElementState.msg b/kortex_driver/msg/ControllerElementState.msg new file mode 100644 index 00000000..23fe0842 --- /dev/null +++ b/kortex_driver/msg/ControllerElementState.msg @@ -0,0 +1,6 @@ + + +ControllerElementHandle handle + +uint32 event_type +float32 axis_value \ No newline at end of file diff --git a/kortex_driver/msg/ControllerEvent.msg b/kortex_driver/msg/ControllerEvent.msg new file mode 100644 index 00000000..3921fb1c --- /dev/null +++ b/kortex_driver/msg/ControllerEvent.msg @@ -0,0 +1,6 @@ + + +uint32 input_type + +uint32 behavior +uint32 input_identifier \ No newline at end of file diff --git a/kortex_driver/msg/ControllerEventType.msg b/kortex_driver/msg/ControllerEventType.msg new file mode 100644 index 00000000..e72f156e --- /dev/null +++ b/kortex_driver/msg/ControllerEventType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_CONTROLLER_EVENT = 0 + +uint32 CONTROLLER_DISCONNECTED = 1 + +uint32 CONTROLLER_CONNECTED = 2 diff --git a/kortex_driver/msg/ControllerHandle.msg b/kortex_driver/msg/ControllerHandle.msg new file mode 100644 index 00000000..96f814df --- /dev/null +++ b/kortex_driver/msg/ControllerHandle.msg @@ -0,0 +1,4 @@ + + +uint32 type +uint32 controller_identifier \ No newline at end of file diff --git a/kortex_driver/msg/ControllerInputType.msg b/kortex_driver/msg/ControllerInputType.msg new file mode 100644 index 00000000..7a448d50 --- /dev/null +++ b/kortex_driver/msg/ControllerInputType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_CONTROLLER_INPUT_TYPE = 0 + +uint32 ANALOG = 1 + +uint32 DIGITAL = 2 diff --git a/kortex_driver/msg/ControllerList.msg b/kortex_driver/msg/ControllerList.msg new file mode 100644 index 00000000..d94f3f0b --- /dev/null +++ b/kortex_driver/msg/ControllerList.msg @@ -0,0 +1,3 @@ + + +ControllerHandle[] handles \ No newline at end of file diff --git a/kortex_driver/msg/ControllerNotification.msg b/kortex_driver/msg/ControllerNotification.msg new file mode 100644 index 00000000..771dca52 --- /dev/null +++ b/kortex_driver/msg/ControllerNotification.msg @@ -0,0 +1,7 @@ + + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ControllerNotificationList.msg b/kortex_driver/msg/ControllerNotificationList.msg new file mode 100644 index 00000000..33b1a336 --- /dev/null +++ b/kortex_driver/msg/ControllerNotificationList.msg @@ -0,0 +1,3 @@ + + +ControllerNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/ControllerState.msg b/kortex_driver/msg/ControllerState.msg new file mode 100644 index 00000000..01ee23ac --- /dev/null +++ b/kortex_driver/msg/ControllerState.msg @@ -0,0 +1,5 @@ + + +ControllerHandle handle + +uint32 event_type \ No newline at end of file diff --git a/kortex_driver/msg/ControllerType.msg b/kortex_driver/msg/ControllerType.msg new file mode 100644 index 00000000..102c28c0 --- /dev/null +++ b/kortex_driver/msg/ControllerType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_CONTROLLER_TYPE = 0 + +uint32 XBOX_CONTROLLER = 1 + +uint32 WRIST_CONTROLLER = 2 diff --git a/kortex_driver/msg/CustomData.msg b/kortex_driver/msg/CustomData.msg new file mode 100644 index 00000000..6714137a --- /dev/null +++ b/kortex_driver/msg/CustomData.msg @@ -0,0 +1,14 @@ + +uint32 frame_id +uint32 custom_data_0 +uint32 custom_data_1 +uint32 custom_data_2 +uint32 custom_data_3 +uint32 custom_data_4 +uint32 custom_data_5 +uint32 custom_data_6 +uint32 custom_data_7 + +ActuatorCustomData[] actuators_custom_data + +InterconnectCustomData interconnect_custom_data \ No newline at end of file diff --git a/kortex_driver/msg/Delay.msg b/kortex_driver/msg/Delay.msg new file mode 100644 index 00000000..dbff7d1e --- /dev/null +++ b/kortex_driver/msg/Delay.msg @@ -0,0 +1,2 @@ + +uint32 duration \ No newline at end of file diff --git a/kortex_driver/msg/DeviceHandle.msg b/kortex_driver/msg/DeviceHandle.msg new file mode 100644 index 00000000..49f84366 --- /dev/null +++ b/kortex_driver/msg/DeviceHandle.msg @@ -0,0 +1,5 @@ + + +uint32 device_type +uint32 device_identifier +uint32 order \ No newline at end of file diff --git a/kortex_driver/msg/DeviceTypes.msg b/kortex_driver/msg/DeviceTypes.msg new file mode 100644 index 00000000..5a55df07 --- /dev/null +++ b/kortex_driver/msg/DeviceTypes.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_DEVICE_TYPE = 0 + +uint32 BASE = 1 + +uint32 VISION = 2 + +uint32 BIG_ACTUATOR = 3 + +uint32 SMALL_ACTUATOR = 4 + +uint32 INTERCONNECT = 5 + +uint32 GRIPPER = 6 diff --git a/kortex_driver/msg/EmergencyStop.msg b/kortex_driver/msg/EmergencyStop.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_driver/msg/Empty.msg b/kortex_driver/msg/Empty.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_driver/msg/ErrorCodes.msg b/kortex_driver/msg/ErrorCodes.msg new file mode 100644 index 00000000..4b9872a6 --- /dev/null +++ b/kortex_driver/msg/ErrorCodes.msg @@ -0,0 +1,10 @@ + +uint32 SUCCESS = 0 + +uint32 ERROR_PROTOCOL_SERVER = 1 + +uint32 ERROR_PROTOCOL_CLIENT = 2 + +uint32 ERROR_DEVICE = 3 + +uint32 ERROR_INTERNAL = 4 diff --git a/kortex_driver/msg/EventIdSequenceInfoNotification.msg b/kortex_driver/msg/EventIdSequenceInfoNotification.msg new file mode 100644 index 00000000..e20013fa --- /dev/null +++ b/kortex_driver/msg/EventIdSequenceInfoNotification.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_SEQUENCE_EVENT = 0 + +uint32 END = 1 + +uint32 ABORT = 2 + +uint32 PAUSE = 3 + +uint32 TASK_START = 4 + +uint32 TASK_END = 5 + +uint32 START = 6 diff --git a/kortex_driver/msg/FactoryEvent.msg b/kortex_driver/msg/FactoryEvent.msg new file mode 100644 index 00000000..ec57bf79 --- /dev/null +++ b/kortex_driver/msg/FactoryEvent.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_FACTORY_EVENT = 0 + +uint32 FACTORY_DEFAULT_RESTORED = 1 + +uint32 NETWORK_FACTORY_DEFAULT_RESTORED = 2 diff --git a/kortex_driver/msg/FactoryNotification.msg b/kortex_driver/msg/FactoryNotification.msg new file mode 100644 index 00000000..40a0c973 --- /dev/null +++ b/kortex_driver/msg/FactoryNotification.msg @@ -0,0 +1,9 @@ + + +uint32 event + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/Faults.msg b/kortex_driver/msg/Faults.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_driver/msg/Feedback.msg b/kortex_driver/msg/Feedback.msg new file mode 100644 index 00000000..be1a6c3f --- /dev/null +++ b/kortex_driver/msg/Feedback.msg @@ -0,0 +1,8 @@ + +uint32 frame_id + +BaseFeedback base + +ActuatorFeedback[] actuators + +InterconnectFeedback interconnect \ No newline at end of file diff --git a/kortex_driver/msg/Finger.msg b/kortex_driver/msg/Finger.msg new file mode 100644 index 00000000..909fac72 --- /dev/null +++ b/kortex_driver/msg/Finger.msg @@ -0,0 +1,3 @@ + +uint32 finger_identifier +float32 value \ No newline at end of file diff --git a/kortex_driver/msg/FullIPv4Configuration.msg b/kortex_driver/msg/FullIPv4Configuration.msg new file mode 100644 index 00000000..f364c6a3 --- /dev/null +++ b/kortex_driver/msg/FullIPv4Configuration.msg @@ -0,0 +1,5 @@ + + +NetworkHandle handle + +IPv4Configuration ipv4_configuration \ No newline at end of file diff --git a/kortex_driver/msg/FullUserProfile.msg b/kortex_driver/msg/FullUserProfile.msg new file mode 100644 index 00000000..22e198c2 --- /dev/null +++ b/kortex_driver/msg/FullUserProfile.msg @@ -0,0 +1,4 @@ + + +UserProfile user_profile +string password \ No newline at end of file diff --git a/kortex_driver/msg/GpioEvent.msg b/kortex_driver/msg/GpioEvent.msg new file mode 100644 index 00000000..4ac2edaf --- /dev/null +++ b/kortex_driver/msg/GpioEvent.msg @@ -0,0 +1,4 @@ + + +uint32 gpio_state +uint32 device_identifier \ No newline at end of file diff --git a/kortex_driver/msg/GpioState.msg b/kortex_driver/msg/GpioState.msg new file mode 100644 index 00000000..4249b74d --- /dev/null +++ b/kortex_driver/msg/GpioState.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_GPIO_STATE = 0 + +uint32 GPIO_OFF = 1 + +uint32 GPIO_PULSE = 2 + +uint32 GPIO_ON = 3 diff --git a/kortex_driver/msg/Gripper.msg b/kortex_driver/msg/Gripper.msg new file mode 100644 index 00000000..73be9791 --- /dev/null +++ b/kortex_driver/msg/Gripper.msg @@ -0,0 +1,3 @@ + + +Finger[] finger \ No newline at end of file diff --git a/kortex_driver/msg/GripperCommand.msg b/kortex_driver/msg/GripperCommand.msg new file mode 100644 index 00000000..d1d91df0 --- /dev/null +++ b/kortex_driver/msg/GripperCommand.msg @@ -0,0 +1,6 @@ + + +uint32 mode + +Gripper gripper +uint32 duration \ No newline at end of file diff --git a/kortex_driver/msg/GripperMode.msg b/kortex_driver/msg/GripperMode.msg new file mode 100644 index 00000000..db6b7288 --- /dev/null +++ b/kortex_driver/msg/GripperMode.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_GRIPPER_MODE = 0 + +uint32 GRIPPER_FORCE = 1 + +uint32 GRIPPER_SPEED = 2 + +uint32 GRIPPER_POSITION = 3 diff --git a/kortex_driver/msg/GripperRequest.msg b/kortex_driver/msg/GripperRequest.msg new file mode 100644 index 00000000..bdde570c --- /dev/null +++ b/kortex_driver/msg/GripperRequest.msg @@ -0,0 +1,3 @@ + + +uint32 mode \ No newline at end of file diff --git a/kortex_driver/msg/IPv4Configuration.msg b/kortex_driver/msg/IPv4Configuration.msg new file mode 100644 index 00000000..dc0cd6a9 --- /dev/null +++ b/kortex_driver/msg/IPv4Configuration.msg @@ -0,0 +1,5 @@ + +uint32 ip_address +uint32 subnet_mask +uint32 default_gateway +bool dhcp_enabled \ No newline at end of file diff --git a/kortex_driver/msg/IPv4Information.msg b/kortex_driver/msg/IPv4Information.msg new file mode 100644 index 00000000..4e95c58c --- /dev/null +++ b/kortex_driver/msg/IPv4Information.msg @@ -0,0 +1,4 @@ + +uint32 ip_address +uint32 subnet_mask +uint32 default_gateway \ No newline at end of file diff --git a/kortex_driver/msg/InterconnectCommand.msg b/kortex_driver/msg/InterconnectCommand.msg new file mode 100644 index 00000000..1b29bef5 --- /dev/null +++ b/kortex_driver/msg/InterconnectCommand.msg @@ -0,0 +1,6 @@ + +uint32 command_id +uint32 flags +float32 position +float32 velocity +float32 force \ No newline at end of file diff --git a/kortex_driver/msg/InterconnectCustomData.msg b/kortex_driver/msg/InterconnectCustomData.msg new file mode 100644 index 00000000..1262a849 --- /dev/null +++ b/kortex_driver/msg/InterconnectCustomData.msg @@ -0,0 +1,18 @@ + +uint32 command_id +uint32 custom_data_0 +uint32 custom_data_1 +uint32 custom_data_2 +uint32 custom_data_3 +uint32 custom_data_4 +uint32 custom_data_5 +uint32 custom_data_6 +uint32 custom_data_7 +uint32 custom_data_8 +uint32 custom_data_9 +uint32 custom_data_10 +uint32 custom_data_11 +uint32 custom_data_12 +uint32 custom_data_13 +uint32 custom_data_14 +uint32 custom_data_15 \ No newline at end of file diff --git a/kortex_driver/msg/InterconnectFeedback.msg b/kortex_driver/msg/InterconnectFeedback.msg new file mode 100644 index 00000000..389a5921 --- /dev/null +++ b/kortex_driver/msg/InterconnectFeedback.msg @@ -0,0 +1,19 @@ + +uint32 command_id +uint32 status_flags +uint32 jitter_comm +float32 position +float32 velocity +float32 force +float32 imu_acceleration_x +float32 imu_acceleration_y +float32 imu_acceleration_z +float32 imu_angular_velocity_x +float32 imu_angular_velocity_y +float32 imu_angular_velocity_z +float32 voltage +float32 temperature_core +uint32 fault_bank_a +uint32 fault_bank_b +uint32 warning_bank_a +uint32 warning_bank_b \ No newline at end of file diff --git a/kortex_driver/msg/JointAngle.msg b/kortex_driver/msg/JointAngle.msg new file mode 100644 index 00000000..1b2a9d13 --- /dev/null +++ b/kortex_driver/msg/JointAngle.msg @@ -0,0 +1,3 @@ + +uint32 joint_identifier +float32 value \ No newline at end of file diff --git a/kortex_driver/msg/JointAngles.msg b/kortex_driver/msg/JointAngles.msg new file mode 100644 index 00000000..fe457738 --- /dev/null +++ b/kortex_driver/msg/JointAngles.msg @@ -0,0 +1,3 @@ + + +JointAngle[] joint_angles \ No newline at end of file diff --git a/kortex_driver/msg/JointLimitation.msg b/kortex_driver/msg/JointLimitation.msg new file mode 100644 index 00000000..b9636ff6 --- /dev/null +++ b/kortex_driver/msg/JointLimitation.msg @@ -0,0 +1,4 @@ + +uint32 device_identifier + +JointLimitationValue limitation_value \ No newline at end of file diff --git a/kortex_driver/msg/JointLimitationTypeIdentifier.msg b/kortex_driver/msg/JointLimitationTypeIdentifier.msg new file mode 100644 index 00000000..64be2d43 --- /dev/null +++ b/kortex_driver/msg/JointLimitationTypeIdentifier.msg @@ -0,0 +1,4 @@ + +uint32 device_identifier + +uint32 type \ No newline at end of file diff --git a/kortex_driver/msg/JointLimitationValue.msg b/kortex_driver/msg/JointLimitationValue.msg new file mode 100644 index 00000000..b096b246 --- /dev/null +++ b/kortex_driver/msg/JointLimitationValue.msg @@ -0,0 +1,4 @@ + + +uint32 type +float32 value \ No newline at end of file diff --git a/kortex_driver/msg/JointLimitationValueList.msg b/kortex_driver/msg/JointLimitationValueList.msg new file mode 100644 index 00000000..a26fc963 --- /dev/null +++ b/kortex_driver/msg/JointLimitationValueList.msg @@ -0,0 +1,3 @@ + + +JointLimitationValue[] joint_limitation_values \ No newline at end of file diff --git a/kortex_driver/msg/JointNavigationDirection.msg b/kortex_driver/msg/JointNavigationDirection.msg new file mode 100644 index 00000000..274085ff --- /dev/null +++ b/kortex_driver/msg/JointNavigationDirection.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_JOINT_NAVIGATION_DIRECTION = 0 + +uint32 JOINT_NEXT = 1 + +uint32 JOINT_PREVIOUS = 2 diff --git a/kortex_driver/msg/JointSpeed.msg b/kortex_driver/msg/JointSpeed.msg new file mode 100644 index 00000000..bcf5540b --- /dev/null +++ b/kortex_driver/msg/JointSpeed.msg @@ -0,0 +1,4 @@ + +uint32 joint_identifier +float32 value +uint32 duration \ No newline at end of file diff --git a/kortex_driver/msg/JointSpeeds.msg b/kortex_driver/msg/JointSpeeds.msg new file mode 100644 index 00000000..f5f77f4f --- /dev/null +++ b/kortex_driver/msg/JointSpeeds.msg @@ -0,0 +1,4 @@ + + +JointSpeed[] joint_speeds +uint32 duration \ No newline at end of file diff --git a/kortex_driver/msg/JointTrajectoryConstraint.msg b/kortex_driver/msg/JointTrajectoryConstraint.msg new file mode 100644 index 00000000..b096b246 --- /dev/null +++ b/kortex_driver/msg/JointTrajectoryConstraint.msg @@ -0,0 +1,4 @@ + + +uint32 type +float32 value \ No newline at end of file diff --git a/kortex_driver/msg/JointTrajectoryConstraintType.msg b/kortex_driver/msg/JointTrajectoryConstraintType.msg new file mode 100644 index 00000000..22b33997 --- /dev/null +++ b/kortex_driver/msg/JointTrajectoryConstraintType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_JOINT_CONSTRAINT = 0 + +uint32 JOINT_CONSTRAINT_DURATION = 1 + +uint32 JOINT_CONSTRAINT_SPEED = 2 diff --git a/kortex_driver/msg/LedState.msg b/kortex_driver/msg/LedState.msg new file mode 100644 index 00000000..93fc79a7 --- /dev/null +++ b/kortex_driver/msg/LedState.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_LED_STATE = 0 + +uint32 LED_OFF = 1 + +uint32 LED_PULSE = 2 + +uint32 LED_ON = 3 diff --git a/kortex_driver/msg/LimitationType.msg b/kortex_driver/msg/LimitationType.msg new file mode 100644 index 00000000..04f709c9 --- /dev/null +++ b/kortex_driver/msg/LimitationType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_LIMITATION = 0 + +uint32 FORCE_LIMITATION = 1 + +uint32 ACCELERATION_LIMITATION = 2 + +uint32 VELOCITY_LIMITATION = 3 diff --git a/kortex_driver/msg/LimitationTypeIdentifier.msg b/kortex_driver/msg/LimitationTypeIdentifier.msg new file mode 100644 index 00000000..9143f866 --- /dev/null +++ b/kortex_driver/msg/LimitationTypeIdentifier.msg @@ -0,0 +1,3 @@ + + +uint32 type \ No newline at end of file diff --git a/kortex_driver/msg/Map.msg b/kortex_driver/msg/Map.msg new file mode 100644 index 00000000..ca25171c --- /dev/null +++ b/kortex_driver/msg/Map.msg @@ -0,0 +1,6 @@ + + +MapHandle handle +string name + +MapElement[] elements \ No newline at end of file diff --git a/kortex_driver/msg/MapElement.msg b/kortex_driver/msg/MapElement.msg new file mode 100644 index 00000000..efa91ae5 --- /dev/null +++ b/kortex_driver/msg/MapElement.msg @@ -0,0 +1,5 @@ + + +MapEvent event + +Action action \ No newline at end of file diff --git a/kortex_driver/msg/MapEvent.msg b/kortex_driver/msg/MapEvent.msg new file mode 100644 index 00000000..269bbdaf --- /dev/null +++ b/kortex_driver/msg/MapEvent.msg @@ -0,0 +1,2 @@ + +string name \ No newline at end of file diff --git a/kortex_driver/msg/MapGroup.msg b/kortex_driver/msg/MapGroup.msg new file mode 100644 index 00000000..64f73094 --- /dev/null +++ b/kortex_driver/msg/MapGroup.msg @@ -0,0 +1,13 @@ + + +MapGroupHandle group_handle +string name + +MappingHandle related_mapping_handle + +MapGroupHandle parent_group_handle + +MapGroupHandle[] children_map_group_handles + +MapHandle[] map_handles +string application_data \ No newline at end of file diff --git a/kortex_driver/msg/MapGroupHandle.msg b/kortex_driver/msg/MapGroupHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/MapGroupHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/MapGroupList.msg b/kortex_driver/msg/MapGroupList.msg new file mode 100644 index 00000000..e2f32b1d --- /dev/null +++ b/kortex_driver/msg/MapGroupList.msg @@ -0,0 +1,3 @@ + + +MapGroup[] map_groups \ No newline at end of file diff --git a/kortex_driver/msg/MapHandle.msg b/kortex_driver/msg/MapHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/MapHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/MapList.msg b/kortex_driver/msg/MapList.msg new file mode 100644 index 00000000..dbcc9900 --- /dev/null +++ b/kortex_driver/msg/MapList.msg @@ -0,0 +1,3 @@ + + +Map[] map_list \ No newline at end of file diff --git a/kortex_driver/msg/Mapping.msg b/kortex_driver/msg/Mapping.msg new file mode 100644 index 00000000..095a9350 --- /dev/null +++ b/kortex_driver/msg/Mapping.msg @@ -0,0 +1,14 @@ + + +MappingHandle handle +string name +uint32 controller_identifier + +MapGroupHandle active_map_group_handle + +MapGroupHandle[] map_group_handles + +MapHandle active_map_handle + +MapHandle[] map_handles +string application_data \ No newline at end of file diff --git a/kortex_driver/msg/MappingHandle.msg b/kortex_driver/msg/MappingHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/MappingHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/MappingInfoNotification.msg b/kortex_driver/msg/MappingInfoNotification.msg new file mode 100644 index 00000000..b33d1de5 --- /dev/null +++ b/kortex_driver/msg/MappingInfoNotification.msg @@ -0,0 +1,10 @@ + +uint32 controller_identifier + +MapHandle active_map_handle + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/MappingInfoNotificationList.msg b/kortex_driver/msg/MappingInfoNotificationList.msg new file mode 100644 index 00000000..07fa3da4 --- /dev/null +++ b/kortex_driver/msg/MappingInfoNotificationList.msg @@ -0,0 +1,3 @@ + + +MappingInfoNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/MappingList.msg b/kortex_driver/msg/MappingList.msg new file mode 100644 index 00000000..b8ac2cce --- /dev/null +++ b/kortex_driver/msg/MappingList.msg @@ -0,0 +1,3 @@ + + +Mapping[] mappings \ No newline at end of file diff --git a/kortex_driver/msg/NavigationDirection.msg b/kortex_driver/msg/NavigationDirection.msg new file mode 100644 index 00000000..93f1c4c8 --- /dev/null +++ b/kortex_driver/msg/NavigationDirection.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_NAVIGATION_DIRECTION = 0 + +uint32 NEXT = 1 + +uint32 UP = 2 + +uint32 DOWN = 3 + +uint32 PREVIOUS = 4 diff --git a/kortex_driver/msg/NetworkEvent.msg b/kortex_driver/msg/NetworkEvent.msg new file mode 100644 index 00000000..217903d0 --- /dev/null +++ b/kortex_driver/msg/NetworkEvent.msg @@ -0,0 +1,22 @@ + +uint32 UNSPECIFIED_NETWORK_EVENT = 0 + +uint32 WIFI_CONNECTED = 1 + +uint32 WIFI_DISCONNECTED = 2 + +uint32 WIFI_SCAN_STARTED = 3 + +uint32 WIFI_SCAN_RESULTS = 4 + +uint32 WIFI_SCAN_FAILED = 5 + +uint32 WIFI_NOT_FOUND = 6 + +uint32 WIFI_ASSOC_REJECTED = 7 + +uint32 WIFI_AUTH_WRONG_KEY = 8 + +uint32 WIFI_AUTH_CONN_FAILED = 9 + +uint32 WIFI_AUTH_FAILED = 10 diff --git a/kortex_driver/msg/NetworkHandle.msg b/kortex_driver/msg/NetworkHandle.msg new file mode 100644 index 00000000..9143f866 --- /dev/null +++ b/kortex_driver/msg/NetworkHandle.msg @@ -0,0 +1,3 @@ + + +uint32 type \ No newline at end of file diff --git a/kortex_driver/msg/NetworkNotification.msg b/kortex_driver/msg/NetworkNotification.msg new file mode 100644 index 00000000..40a0c973 --- /dev/null +++ b/kortex_driver/msg/NetworkNotification.msg @@ -0,0 +1,9 @@ + + +uint32 event + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/NetworkNotificationList.msg b/kortex_driver/msg/NetworkNotificationList.msg new file mode 100644 index 00000000..49053108 --- /dev/null +++ b/kortex_driver/msg/NetworkNotificationList.msg @@ -0,0 +1,3 @@ + + +NetworkNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/NetworkType.msg b/kortex_driver/msg/NetworkType.msg new file mode 100644 index 00000000..ec8fc9c0 --- /dev/null +++ b/kortex_driver/msg/NetworkType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_NETWORK_TYPE = 0 + +uint32 WIFI = 1 + +uint32 WIRED_ETHERNET = 2 diff --git a/kortex_driver/msg/NotificationHandle.msg b/kortex_driver/msg/NotificationHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_driver/msg/NotificationHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_driver/msg/NotificationOptions.msg b/kortex_driver/msg/NotificationOptions.msg new file mode 100644 index 00000000..6ff46842 --- /dev/null +++ b/kortex_driver/msg/NotificationOptions.msg @@ -0,0 +1,5 @@ + + +uint32 type +uint32 rate_m_sec +float32 threshold_value \ No newline at end of file diff --git a/kortex_driver/msg/NotificationType.msg b/kortex_driver/msg/NotificationType.msg new file mode 100644 index 00000000..79dd058c --- /dev/null +++ b/kortex_driver/msg/NotificationType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_NOTIFICATION_TYPE = 0 + +uint32 THRESHOLD = 1 + +uint32 FIX_RATE = 2 + +uint32 EVENT = 3 diff --git a/kortex_driver/msg/OperatingMode.msg b/kortex_driver/msg/OperatingMode.msg new file mode 100644 index 00000000..c2fc2bde --- /dev/null +++ b/kortex_driver/msg/OperatingMode.msg @@ -0,0 +1,16 @@ + +uint32 UNSPECIFIED_OPERATING_MODE = 0 + +uint32 MAINTENANCE_MODE = 1 + +uint32 UPDATE_MODE = 2 + +uint32 UPDATE_COMPLETED_MODE = 3 + +uint32 UPDATE_FAILED_MODE = 4 + +uint32 SHUTTING_DOWN_MODE = 5 + +uint32 RUN_MODE = 6 + +uint32 UPDATING_DEVICE_MODE = 7 diff --git a/kortex_driver/msg/OperatingModeInformation.msg b/kortex_driver/msg/OperatingModeInformation.msg new file mode 100644 index 00000000..8b769914 --- /dev/null +++ b/kortex_driver/msg/OperatingModeInformation.msg @@ -0,0 +1,5 @@ + + +uint32 operating_mode + +DeviceHandle device_handle \ No newline at end of file diff --git a/kortex_driver/msg/OperatingModeNotification.msg b/kortex_driver/msg/OperatingModeNotification.msg new file mode 100644 index 00000000..ecb91fc4 --- /dev/null +++ b/kortex_driver/msg/OperatingModeNotification.msg @@ -0,0 +1,11 @@ + + +uint32 operating_mode + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection + +DeviceHandle device_handle \ No newline at end of file diff --git a/kortex_driver/msg/OperatingModeNotificationList.msg b/kortex_driver/msg/OperatingModeNotificationList.msg new file mode 100644 index 00000000..18380ec0 --- /dev/null +++ b/kortex_driver/msg/OperatingModeNotificationList.msg @@ -0,0 +1,3 @@ + + +OperatingModeNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/Orientation.msg b/kortex_driver/msg/Orientation.msg new file mode 100644 index 00000000..6f06d804 --- /dev/null +++ b/kortex_driver/msg/Orientation.msg @@ -0,0 +1,4 @@ + +float32 theta_x +float32 theta_y +float32 theta_z \ No newline at end of file diff --git a/kortex_driver/msg/PasswordChange.msg b/kortex_driver/msg/PasswordChange.msg new file mode 100644 index 00000000..57ee47e3 --- /dev/null +++ b/kortex_driver/msg/PasswordChange.msg @@ -0,0 +1,5 @@ + + +UserProfileHandle handle +string old_password +string new_password \ No newline at end of file diff --git a/kortex_driver/msg/Permission.msg b/kortex_driver/msg/Permission.msg new file mode 100644 index 00000000..c5399e9c --- /dev/null +++ b/kortex_driver/msg/Permission.msg @@ -0,0 +1,8 @@ + +uint32 NO_PERMISSION = 0 + +uint32 READ_PERMISSION = 1 + +uint32 UPDATE_PERMISSION = 2 + +uint32 DELETE_PERMISSION = 4 diff --git a/kortex_driver/msg/Point.msg b/kortex_driver/msg/Point.msg new file mode 100644 index 00000000..78f07ff5 --- /dev/null +++ b/kortex_driver/msg/Point.msg @@ -0,0 +1,4 @@ + +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/kortex_driver/msg/Pose.msg b/kortex_driver/msg/Pose.msg new file mode 100644 index 00000000..beddec03 --- /dev/null +++ b/kortex_driver/msg/Pose.msg @@ -0,0 +1,7 @@ + +float32 x +float32 y +float32 z +float32 theta_x +float32 theta_y +float32 theta_z \ No newline at end of file diff --git a/kortex_driver/msg/Position.msg b/kortex_driver/msg/Position.msg new file mode 100644 index 00000000..78f07ff5 --- /dev/null +++ b/kortex_driver/msg/Position.msg @@ -0,0 +1,4 @@ + +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZone.msg b/kortex_driver/msg/ProtectionZone.msg new file mode 100644 index 00000000..5bf1b8e5 --- /dev/null +++ b/kortex_driver/msg/ProtectionZone.msg @@ -0,0 +1,12 @@ + + +ProtectionZoneHandle handle +string name +string application_data +bool is_enabled + +ZoneShape shape + +CartesianLimitation[] limitations + +CartesianLimitation[] envelope_limitations \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZoneEvent.msg b/kortex_driver/msg/ProtectionZoneEvent.msg new file mode 100644 index 00000000..382e487f --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneEvent.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_PROTECTION_ZONE_EVENT = 0 + +uint32 REACHED = 1 + +uint32 ENTERED = 2 + +uint32 EXITED = 3 diff --git a/kortex_driver/msg/ProtectionZoneHandle.msg b/kortex_driver/msg/ProtectionZoneHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZoneInformation.msg b/kortex_driver/msg/ProtectionZoneInformation.msg new file mode 100644 index 00000000..c117fb78 --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneInformation.msg @@ -0,0 +1,3 @@ + + +uint32 event \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZoneList.msg b/kortex_driver/msg/ProtectionZoneList.msg new file mode 100644 index 00000000..978122e0 --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneList.msg @@ -0,0 +1,3 @@ + + +ProtectionZone[] protection_zones \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZoneNotification.msg b/kortex_driver/msg/ProtectionZoneNotification.msg new file mode 100644 index 00000000..0535cb61 --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneNotification.msg @@ -0,0 +1,11 @@ + + +uint32 event + +ProtectionZoneHandle handle + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ProtectionZoneNotificationList.msg b/kortex_driver/msg/ProtectionZoneNotificationList.msg new file mode 100644 index 00000000..8744bf04 --- /dev/null +++ b/kortex_driver/msg/ProtectionZoneNotificationList.msg @@ -0,0 +1,3 @@ + + +ProtectionZoneNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/Query.msg b/kortex_driver/msg/Query.msg new file mode 100644 index 00000000..b8c2a292 --- /dev/null +++ b/kortex_driver/msg/Query.msg @@ -0,0 +1,6 @@ + + +Timestamp start_timestamp + +Timestamp end_timestamp +string username \ No newline at end of file diff --git a/kortex_driver/msg/RequestedActionType.msg b/kortex_driver/msg/RequestedActionType.msg new file mode 100644 index 00000000..05c86522 --- /dev/null +++ b/kortex_driver/msg/RequestedActionType.msg @@ -0,0 +1,3 @@ + + +uint32 action_type \ No newline at end of file diff --git a/kortex_driver/msg/RobotEvent.msg b/kortex_driver/msg/RobotEvent.msg new file mode 100644 index 00000000..2174cd2d --- /dev/null +++ b/kortex_driver/msg/RobotEvent.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_ROBOT_EVENT = 0 + +uint32 ARM_CONNECTED = 1 + +uint32 ARM_DISCONNECTED = 2 + +uint32 TOOL_CONNECTED = 5 + +uint32 TOOL_DISCONNECTED = 6 diff --git a/kortex_driver/msg/RobotEventNotification.msg b/kortex_driver/msg/RobotEventNotification.msg new file mode 100644 index 00000000..dc2cfd1e --- /dev/null +++ b/kortex_driver/msg/RobotEventNotification.msg @@ -0,0 +1,11 @@ + + +uint32 event + +DeviceHandle handle + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/RobotEventNotificationList.msg b/kortex_driver/msg/RobotEventNotificationList.msg new file mode 100644 index 00000000..4ecd1889 --- /dev/null +++ b/kortex_driver/msg/RobotEventNotificationList.msg @@ -0,0 +1,3 @@ + + +RobotEventNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/RotationMatrix.msg b/kortex_driver/msg/RotationMatrix.msg new file mode 100644 index 00000000..70a31f14 --- /dev/null +++ b/kortex_driver/msg/RotationMatrix.msg @@ -0,0 +1,7 @@ + + +RotationMatrixRow row1 + +RotationMatrixRow row2 + +RotationMatrixRow row3 \ No newline at end of file diff --git a/kortex_driver/msg/RotationMatrixRow.msg b/kortex_driver/msg/RotationMatrixRow.msg new file mode 100644 index 00000000..39a30f92 --- /dev/null +++ b/kortex_driver/msg/RotationMatrixRow.msg @@ -0,0 +1,4 @@ + +float32 column1 +float32 column2 +float32 column3 \ No newline at end of file diff --git a/kortex_driver/msg/SafetyEvent.msg b/kortex_driver/msg/SafetyEvent.msg new file mode 100644 index 00000000..4c5bd430 --- /dev/null +++ b/kortex_driver/msg/SafetyEvent.msg @@ -0,0 +1,3 @@ + + +SafetyHandle safety_handle \ No newline at end of file diff --git a/kortex_driver/msg/SafetyHandle.msg b/kortex_driver/msg/SafetyHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_driver/msg/SafetyHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_driver/msg/SafetyIdentifier.msg b/kortex_driver/msg/SafetyIdentifier.msg new file mode 100644 index 00000000..1a9b56d4 --- /dev/null +++ b/kortex_driver/msg/SafetyIdentifier.msg @@ -0,0 +1,60 @@ + +uint32 UNSPECIFIED_BASE_SAFETY_IDENTIFIER = 0 + +uint32 FIRMWARE_UPDATE_FAILURE = 1 + +uint32 EXTERNAL_COMMUNICATION_ERROR = 2 + +uint32 MAXIMUM_AMBIENT_TEMPERATURE = 4 + +uint32 MAXIMUM_CORE_TEMPERATURE = 8 + +uint32 JOINT_FAULT = 16 + +uint32 CYCLIC_DATA_JITTER = 32 + +uint32 REACHED_MAXIMUM_EVENT_LOGS = 64 + +uint32 NO_KINEMATICS_SUPPORT = 128 + +uint32 ABOVE_MAXIMUM_DOF = 256 + +uint32 NETWORK_ERROR = 512 + +uint32 UNABLE_TO_REACH_POSE = 1024 + +uint32 JOINT_DETECTION_ERROR = 2048 + +uint32 NETWORK_INITIALIZATION_ERROR = 4096 + +uint32 MAXIMUM_CURRENT = 8192 + +uint32 MAXIMUM_VOLTAGE = 16384 + +uint32 MINIMUM_VOLTAGE = 32768 + +uint32 MAXIMUM_END_EFFECTOR_TRANSLATION_VELOCITY = 65536 + +uint32 MAXIMUM_END_EFFECTOR_ORIENTATION_VELOCITY = 131072 + +uint32 MAXIMUM_END_EFFECTOR_TRANSLATION_ACCELERATION = 262144 + +uint32 MAXIMUM_END_EFFECTOR_ORIENTATION_ACCELERATION = 524288 + +uint32 MAXIMUM_END_EFFECTOR_TRANSLATION_FORCE = 1048576 + +uint32 MAXIMUM_END_EFFECTOR_ORIENTATION_FORCE = 2097152 + +uint32 MAXIMUM_END_EFFECTOR_PAYLOAD = 4194304 + +uint32 EMERGENCY_STOP_ACTIVATED = 8388608 + +uint32 EMERGENCY_LINE_ACTIVATED = 16777216 + +uint32 INRUSH_CURRENT_LIMITER_FAULT = 33554432 + +uint32 NVRAM_CORRUPTED = 67108864 + +uint32 INCOMPATIBLE_FIRMWARE_VERSION = 134217728 + +uint32 POWERON_SELF_TEST_FAILURE = 268435456 diff --git a/kortex_driver/msg/SafetyNotification.msg b/kortex_driver/msg/SafetyNotification.msg new file mode 100644 index 00000000..36d88894 --- /dev/null +++ b/kortex_driver/msg/SafetyNotification.msg @@ -0,0 +1,11 @@ + + +SafetyHandle safety_handle + +uint32 value + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/SafetyNotificationList.msg b/kortex_driver/msg/SafetyNotificationList.msg new file mode 100644 index 00000000..95a25fef --- /dev/null +++ b/kortex_driver/msg/SafetyNotificationList.msg @@ -0,0 +1,3 @@ + + +SafetyNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/SafetyStatusValue.msg b/kortex_driver/msg/SafetyStatusValue.msg new file mode 100644 index 00000000..2ea80554 --- /dev/null +++ b/kortex_driver/msg/SafetyStatusValue.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED = 0 + +uint32 WARNING = 1 + +uint32 ERROR = 2 + +uint32 NORMAL = 3 diff --git a/kortex_driver/msg/Sequence.msg b/kortex_driver/msg/Sequence.msg new file mode 100644 index 00000000..5d5a38cf --- /dev/null +++ b/kortex_driver/msg/Sequence.msg @@ -0,0 +1,7 @@ + + +SequenceHandle handle +string name +string application_data + +SequenceTask[] tasks \ No newline at end of file diff --git a/kortex_driver/msg/SequenceHandle.msg b/kortex_driver/msg/SequenceHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/SequenceHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/SequenceInfoNotification.msg b/kortex_driver/msg/SequenceInfoNotification.msg new file mode 100644 index 00000000..c43d59d3 --- /dev/null +++ b/kortex_driver/msg/SequenceInfoNotification.msg @@ -0,0 +1,15 @@ + + +uint32 event_identifier + +SequenceHandle sequence_handle +uint32 task_index +uint32 group_identifier + +Timestamp timestamp + +UserProfileHandle user_handle + +uint32 abort_details + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/SequenceInfoNotificationList.msg b/kortex_driver/msg/SequenceInfoNotificationList.msg new file mode 100644 index 00000000..94ca3844 --- /dev/null +++ b/kortex_driver/msg/SequenceInfoNotificationList.msg @@ -0,0 +1,3 @@ + + +SequenceInfoNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/SequenceInformation.msg b/kortex_driver/msg/SequenceInformation.msg new file mode 100644 index 00000000..b85d43f4 --- /dev/null +++ b/kortex_driver/msg/SequenceInformation.msg @@ -0,0 +1,5 @@ + + +uint32 event_identifier +uint32 task_index +uint32 task_identifier \ No newline at end of file diff --git a/kortex_driver/msg/SequenceList.msg b/kortex_driver/msg/SequenceList.msg new file mode 100644 index 00000000..4b230f7a --- /dev/null +++ b/kortex_driver/msg/SequenceList.msg @@ -0,0 +1,3 @@ + + +Sequence[] sequence_list \ No newline at end of file diff --git a/kortex_driver/msg/SequenceTask.msg b/kortex_driver/msg/SequenceTask.msg new file mode 100644 index 00000000..9632764f --- /dev/null +++ b/kortex_driver/msg/SequenceTask.msg @@ -0,0 +1,5 @@ + +uint32 group_identifier + +Action action +string application_data \ No newline at end of file diff --git a/kortex_driver/msg/SequenceTaskHandle.msg b/kortex_driver/msg/SequenceTaskHandle.msg new file mode 100644 index 00000000..d982d9f3 --- /dev/null +++ b/kortex_driver/msg/SequenceTaskHandle.msg @@ -0,0 +1,4 @@ + + +SequenceHandle sequence_handle +uint32 task_index \ No newline at end of file diff --git a/kortex_driver/msg/ServiceVersion.msg b/kortex_driver/msg/ServiceVersion.msg new file mode 100644 index 00000000..9665d1c2 --- /dev/null +++ b/kortex_driver/msg/ServiceVersion.msg @@ -0,0 +1,4 @@ + +uint32 RESERVED_0 = 0 + +uint32 CURRENT_VERSION = 1 diff --git a/kortex_driver/msg/ServoingMode.msg b/kortex_driver/msg/ServoingMode.msg new file mode 100644 index 00000000..0456b3af --- /dev/null +++ b/kortex_driver/msg/ServoingMode.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_SERVOING_MODE = 0 + +uint32 MULTI_LEVEL_SERVOING = 1 + +uint32 SINGLE_LEVEL_SERVOING = 2 + +uint32 LOW_LEVEL_SERVOING = 3 + +uint32 BYPASS_SERVOING = 4 diff --git a/kortex_driver/msg/ServoingModeInformation.msg b/kortex_driver/msg/ServoingModeInformation.msg new file mode 100644 index 00000000..70b5dbc8 --- /dev/null +++ b/kortex_driver/msg/ServoingModeInformation.msg @@ -0,0 +1,3 @@ + + +uint32 servoing_mode \ No newline at end of file diff --git a/kortex_driver/msg/ServoingModeNotification.msg b/kortex_driver/msg/ServoingModeNotification.msg new file mode 100644 index 00000000..1519538b --- /dev/null +++ b/kortex_driver/msg/ServoingModeNotification.msg @@ -0,0 +1,9 @@ + + +uint32 servoing_mode + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/ServoingModeNotificationList.msg b/kortex_driver/msg/ServoingModeNotificationList.msg new file mode 100644 index 00000000..0fb0e599 --- /dev/null +++ b/kortex_driver/msg/ServoingModeNotificationList.msg @@ -0,0 +1,3 @@ + + +ServoingModeNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/ShapeType.msg b/kortex_driver/msg/ShapeType.msg new file mode 100644 index 00000000..cc4902fb --- /dev/null +++ b/kortex_driver/msg/ShapeType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_SHAPE = 0 + +uint32 CYLINDER = 1 + +uint32 SPHERE = 2 + +uint32 RECTANGULAR_PRISM = 3 diff --git a/kortex_driver/msg/SignalQuality.msg b/kortex_driver/msg/SignalQuality.msg new file mode 100644 index 00000000..fc5ddf18 --- /dev/null +++ b/kortex_driver/msg/SignalQuality.msg @@ -0,0 +1,12 @@ + +uint32 UNSPECIFIED_SIGNAL_QUALITY = 0 + +uint32 POOR = 1 + +uint32 FAIR = 2 + +uint32 GOOD = 3 + +uint32 EXCELLENT = 4 + +uint32 NONE = 5 diff --git a/kortex_driver/msg/SoundType.msg b/kortex_driver/msg/SoundType.msg new file mode 100644 index 00000000..cb95ecb2 --- /dev/null +++ b/kortex_driver/msg/SoundType.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_SOUND_TYPE = 0 + +uint32 BIP_SERIES = 1 + +uint32 SINGLE_BIP = 2 diff --git a/kortex_driver/msg/Ssid.msg b/kortex_driver/msg/Ssid.msg new file mode 100644 index 00000000..91ad8a05 --- /dev/null +++ b/kortex_driver/msg/Ssid.msg @@ -0,0 +1,2 @@ + +string identifier \ No newline at end of file diff --git a/kortex_driver/msg/Stop.msg b/kortex_driver/msg/Stop.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_driver/msg/SubErrorCodes.msg b/kortex_driver/msg/SubErrorCodes.msg new file mode 100644 index 00000000..b68ca3cd --- /dev/null +++ b/kortex_driver/msg/SubErrorCodes.msg @@ -0,0 +1,168 @@ + +uint32 SUB_SUCCESS = 0 + +uint32 FAILED = 1 + +uint32 UNIMPLEMENTED = 2 + +uint32 INVALID_PARAM = 3 + +uint32 UNSUPPORTED_SERVICE = 4 + +uint32 UNSUPPORTED_METHOD = 5 + +uint32 TOO_LARGE_ENCODED_FRAME_BUFFER = 6 + +uint32 FRAME_ENCODING_ERR = 7 + +uint32 FRAME_DECODING_ERR = 8 + +uint32 INCOMPATIBLE_HEADER_VERSION = 9 + +uint32 UNSUPPORTED_FRAME_TYPE = 10 + +uint32 UNREGISTERED_NOTIFICATION_RECEIVED = 11 + +uint32 INVALID_SESSION = 12 + +uint32 PAYLOAD_DECODING_ERR = 13 + +uint32 UNREGISTERED_FRAME_RECEIVED = 14 + +uint32 INVALID_PASSWORD = 15 + +uint32 USER_NOT_FOUND = 16 + +uint32 ENTITY_NOT_FOUND = 17 + +uint32 ROBOT_MOVEMENT_IN_PROGRESS = 18 + +uint32 ROBOT_NOT_MOVING = 19 + +uint32 NO_MORE_STORAGE_SPACE = 20 + +uint32 ROBOT_NOT_READY = 21 + +uint32 ROBOT_IN_FAULT = 22 + +uint32 ROBOT_IN_MAINTENANCE = 23 + +uint32 ROBOT_IN_UPDATE_MODE = 24 + +uint32 ROBOT_IN_EMERGENCY_STOP = 25 + +uint32 SINGLE_LEVEL_SERVOING = 26 + +uint32 LOW_LEVEL_SERVOING = 27 + +uint32 MAPPING_GROUP_NON_ROOT = 28 + +uint32 MAPPING_INVALID_GROUP = 29 + +uint32 MAPPING_INVALID_MAP = 30 + +uint32 MAP_GROUP_INVALID_MAP = 31 + +uint32 MAP_GROUP_INVALID_PARENT = 32 + +uint32 MAP_GROUP_INVALID_CHILD = 33 + +uint32 MAP_GROUP_INVALID_MOVE = 34 + +uint32 MAP_IN_USE = 35 + +uint32 WIFI_CONNECT_ERROR = 36 + +uint32 UNSUPPORTED_NETWORK_TYPE = 37 + +uint32 TOO_LARGE_ENCODED_PAYLOAD_BUFFER = 38 + +uint32 UPDATE_PERMISSION_DENIED = 39 + +uint32 DELETE_PERMISSION_DENIED = 40 + +uint32 DATABASE_ERROR = 41 + +uint32 UNSUPPORTED_OPTION = 42 + +uint32 UNSUPPORTED_RESOLUTION = 43 + +uint32 UNSUPPORTED_FRAME_RATE = 44 + +uint32 UNSUPPORTED_BIT_RATE = 45 + +uint32 UNSUPPORTED_ACTION = 46 + +uint32 UNSUPPORTED_FOCUS_ACTION = 47 + +uint32 VALUE_IS_ABOVE_MAXIMUM = 48 + +uint32 VALUE_IS_BELOW_MINIMUM = 49 + +uint32 DEVICE_DISCONNECTED = 50 + +uint32 DEVICE_NOT_READY = 51 + +uint32 INVALID_DEVICE = 52 + +uint32 SAFETY_THRESHOLD_REACHED = 53 + +uint32 INVALID_USER_SESSION_ACCESS = 54 + +uint32 CONTROL_MANUAL_STOP = 55 + +uint32 CONTROL_OUTSIDE_WORKSPACE = 56 + +uint32 CONTROL_ACTUATOR_COUNT_MISMATCH = 57 + +uint32 CONTROL_INVALID_DURATION = 58 + +uint32 CONTROL_INVALID_SPEED = 59 + +uint32 CONTROL_LARGE_SPEED = 60 + +uint32 CONTROL_INVALID_ACCELERATION = 61 + +uint32 CONTROL_INVALID_TIME_STEP = 62 + +uint32 CONTROL_LARGE_SIZE = 63 + +uint32 CONTROL_WRONG_MODE = 64 + +uint32 CONTROL_JOINT_POSITION_LIMIT = 65 + +uint32 CONTROL_NO_FILE_IN_MEMORY = 66 + +uint32 CONTROL_INDEX_OUT_OF_TRAJECTORY = 67 + +uint32 CONTROL_ALREADY_RUNNING = 68 + +uint32 CONTROL_WRONG_STARTING_POINT = 69 + +uint32 CONTROL_CARTESIAN_CANNOT_START = 70 + +uint32 CONTROL_UNDEFINED_CONSTRAINT = 71 + +uint32 CONTROL_UNINITIALIZED = 72 + +uint32 CONTROL_NO_ACTION = 73 + +uint32 CONTROL_UNDEFINED = 74 + +uint32 WRONG_SERVOING_MODE = 75 + +uint32 USERNAME_LENGTH_EXCEEDED = 100 + +uint32 FIRSTNAME_LENGTH_EXCEEDED = 101 + +uint32 LASTNAME_LENGTH_EXCEEDED = 102 + +uint32 PASSWORD_LENGTH_EXCEEDED = 103 + +uint32 USERNAME_ALREADY_EXISTS = 104 + +uint32 USERNAME_EMPTY = 105 + +uint32 PASSWORD_NOT_CHANGED = 106 + +uint32 MAXIMUM_USER_PROFILES_USED = 107 diff --git a/kortex_driver/msg/SwitchControlMapping.msg b/kortex_driver/msg/SwitchControlMapping.msg new file mode 100644 index 00000000..9fe33f79 --- /dev/null +++ b/kortex_driver/msg/SwitchControlMapping.msg @@ -0,0 +1,6 @@ + +uint32 controller_identifier + +MapGroupHandle map_group_handle + +MapHandle map_handle \ No newline at end of file diff --git a/kortex_driver/msg/SystemTime.msg b/kortex_driver/msg/SystemTime.msg new file mode 100644 index 00000000..28c8d69b --- /dev/null +++ b/kortex_driver/msg/SystemTime.msg @@ -0,0 +1,7 @@ + +uint32 sec +uint32 min +uint32 hour +uint32 mday +uint32 mon +uint32 year \ No newline at end of file diff --git a/kortex_driver/msg/Timeout.msg b/kortex_driver/msg/Timeout.msg new file mode 100644 index 00000000..3cf3925c --- /dev/null +++ b/kortex_driver/msg/Timeout.msg @@ -0,0 +1,2 @@ + +uint32 value \ No newline at end of file diff --git a/kortex_driver/msg/Timestamp.msg b/kortex_driver/msg/Timestamp.msg new file mode 100644 index 00000000..5e60508c --- /dev/null +++ b/kortex_driver/msg/Timestamp.msg @@ -0,0 +1,3 @@ + +uint32 sec +uint32 usec \ No newline at end of file diff --git a/kortex_driver/msg/TransformationMatrix.msg b/kortex_driver/msg/TransformationMatrix.msg new file mode 100644 index 00000000..4f70ae6a --- /dev/null +++ b/kortex_driver/msg/TransformationMatrix.msg @@ -0,0 +1,9 @@ + + +TransformationRow r0 + +TransformationRow r1 + +TransformationRow r2 + +TransformationRow r3 \ No newline at end of file diff --git a/kortex_driver/msg/TransformationRow.msg b/kortex_driver/msg/TransformationRow.msg new file mode 100644 index 00000000..b930b732 --- /dev/null +++ b/kortex_driver/msg/TransformationRow.msg @@ -0,0 +1,5 @@ + +float32 c0 +float32 c1 +float32 c2 +float32 c3 \ No newline at end of file diff --git a/kortex_driver/msg/Twist.msg b/kortex_driver/msg/Twist.msg new file mode 100644 index 00000000..856383d5 --- /dev/null +++ b/kortex_driver/msg/Twist.msg @@ -0,0 +1,7 @@ + +float32 linear_x +float32 linear_y +float32 linear_z +float32 angular_x +float32 angular_y +float32 angular_z \ No newline at end of file diff --git a/kortex_driver/msg/TwistCommand.msg b/kortex_driver/msg/TwistCommand.msg new file mode 100644 index 00000000..599db8e2 --- /dev/null +++ b/kortex_driver/msg/TwistCommand.msg @@ -0,0 +1,6 @@ + + +uint32 mode + +Twist twist +uint32 duration \ No newline at end of file diff --git a/kortex_driver/msg/TwistMode.msg b/kortex_driver/msg/TwistMode.msg new file mode 100644 index 00000000..086bad8c --- /dev/null +++ b/kortex_driver/msg/TwistMode.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_TWIST_MODE = 0 + +uint32 TWIST_TOOL_JOYSTICK = 1 + +uint32 TWIST_BASE_FRAME_JOYSTICK = 2 + +uint32 TWIST_CARTESIAN_JOYSTICK = 3 diff --git a/kortex_driver/msg/Unit.msg b/kortex_driver/msg/Unit.msg new file mode 100644 index 00000000..c019aeba --- /dev/null +++ b/kortex_driver/msg/Unit.msg @@ -0,0 +1,28 @@ + +uint32 UNSPECIFIED_UNIT = 0 + +uint32 CELSIUS = 1 + +uint32 AMPERE = 2 + +uint32 VOLT = 3 + +uint32 METER_PER_SECOND = 4 + +uint32 DEGREE_PER_SECOND = 5 + +uint32 METER_PER_SECOND_2 = 6 + +uint32 DEGREE_PER_SECOND_2 = 7 + +uint32 NEWTON = 8 + +uint32 NEWTON_METER = 9 + +uint32 KILOGRAM = 10 + +uint32 DEGREE = 11 + +uint32 TICK = 12 + +uint32 DEGREE_PER_MILLISECOND = 13 diff --git a/kortex_driver/msg/UserEvent.msg b/kortex_driver/msg/UserEvent.msg new file mode 100644 index 00000000..84356e05 --- /dev/null +++ b/kortex_driver/msg/UserEvent.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_USER_EVENT = 0 + +uint32 LOGGED_OUT = 1 + +uint32 LOGGED_IN = 2 diff --git a/kortex_driver/msg/UserList.msg b/kortex_driver/msg/UserList.msg new file mode 100644 index 00000000..1b541bd8 --- /dev/null +++ b/kortex_driver/msg/UserList.msg @@ -0,0 +1,3 @@ + + +UserProfileHandle[] user_handles \ No newline at end of file diff --git a/kortex_driver/msg/UserNotification.msg b/kortex_driver/msg/UserNotification.msg new file mode 100644 index 00000000..667cd0f5 --- /dev/null +++ b/kortex_driver/msg/UserNotification.msg @@ -0,0 +1,11 @@ + + +uint32 user_event + +UserProfileHandle modified_user + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/UserNotificationList.msg b/kortex_driver/msg/UserNotificationList.msg new file mode 100644 index 00000000..f3967773 --- /dev/null +++ b/kortex_driver/msg/UserNotificationList.msg @@ -0,0 +1,3 @@ + + +UserNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/UserProfile.msg b/kortex_driver/msg/UserProfile.msg new file mode 100644 index 00000000..dcaa7b43 --- /dev/null +++ b/kortex_driver/msg/UserProfile.msg @@ -0,0 +1,7 @@ + + +UserProfileHandle handle +string username +string firstname +string lastname +string application_data \ No newline at end of file diff --git a/kortex_driver/msg/UserProfileHandle.msg b/kortex_driver/msg/UserProfileHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_driver/msg/UserProfileHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_driver/msg/UserProfileList.msg b/kortex_driver/msg/UserProfileList.msg new file mode 100644 index 00000000..e9f7fc40 --- /dev/null +++ b/kortex_driver/msg/UserProfileList.msg @@ -0,0 +1,3 @@ + + +UserProfile[] user_profiles \ No newline at end of file diff --git a/kortex_driver/msg/WifiConfiguration.msg b/kortex_driver/msg/WifiConfiguration.msg new file mode 100644 index 00000000..dae3b4e4 --- /dev/null +++ b/kortex_driver/msg/WifiConfiguration.msg @@ -0,0 +1,5 @@ + + +Ssid ssid +string security_key +bool connect_automatically \ No newline at end of file diff --git a/kortex_driver/msg/WifiConfigurationList.msg b/kortex_driver/msg/WifiConfigurationList.msg new file mode 100644 index 00000000..c2734e74 --- /dev/null +++ b/kortex_driver/msg/WifiConfigurationList.msg @@ -0,0 +1,3 @@ + + +WifiConfiguration[] wifi_configuration_list \ No newline at end of file diff --git a/kortex_driver/msg/WifiEncryptionType.msg b/kortex_driver/msg/WifiEncryptionType.msg new file mode 100644 index 00000000..29abf12d --- /dev/null +++ b/kortex_driver/msg/WifiEncryptionType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_ENCRYPTION = 0 + +uint32 AES_ENCRYPTION = 1 + +uint32 TKIP_ENCRYPTION = 2 + +uint32 WEP_ENCRYPTION = 4 diff --git a/kortex_driver/msg/WifiInformation.msg b/kortex_driver/msg/WifiInformation.msg new file mode 100644 index 00000000..8b1d3143 --- /dev/null +++ b/kortex_driver/msg/WifiInformation.msg @@ -0,0 +1,10 @@ + + +Ssid ssid +uint32 security_type +uint32 encryption_type + +uint32 signal_quality +int32 signal_strength +uint32 frequency +uint32 channel \ No newline at end of file diff --git a/kortex_driver/msg/WifiInformationList.msg b/kortex_driver/msg/WifiInformationList.msg new file mode 100644 index 00000000..e1f2114e --- /dev/null +++ b/kortex_driver/msg/WifiInformationList.msg @@ -0,0 +1,3 @@ + + +WifiInformation[] wifi_information_list \ No newline at end of file diff --git a/kortex_driver/msg/WifiSecurityType.msg b/kortex_driver/msg/WifiSecurityType.msg new file mode 100644 index 00000000..9f118bc3 --- /dev/null +++ b/kortex_driver/msg/WifiSecurityType.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_AUTHENTICATION = 0 + +uint32 WEP = 1 + +uint32 WPA2_PERSONAL = 2 + +uint32 WPA_PERSONAL = 4 + +uint32 NO_AUTHENTICATION = 8 diff --git a/kortex_driver/msg/Xbox360AnalogInputIdentifier.msg b/kortex_driver/msg/Xbox360AnalogInputIdentifier.msg new file mode 100644 index 00000000..6946dc3b --- /dev/null +++ b/kortex_driver/msg/Xbox360AnalogInputIdentifier.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_XBOX360_ANALOG = 0 + +uint32 XBOX360_THUMB_LEFT_X = 1 + +uint32 XBOX360_THUMB_LEFT_Y = 2 + +uint32 XBOX360_THUMB_RIGHT_X = 3 + +uint32 XBOX360_THUMB_RIGHT_Y = 4 + +uint32 XBOX360_TRIGGER_LEFT = 5 + +uint32 XBOX360_TRIGGER_RIGHT = 6 diff --git a/kortex_driver/msg/Xbox360DigitalInputIdentifier.msg b/kortex_driver/msg/Xbox360DigitalInputIdentifier.msg new file mode 100644 index 00000000..39e77204 --- /dev/null +++ b/kortex_driver/msg/Xbox360DigitalInputIdentifier.msg @@ -0,0 +1,30 @@ + +uint32 UNSPECIFIED_XBOX360_DIGITAL = 0 + +uint32 XBOX360_PAD_UP = 1 + +uint32 XBOX360_PAD_DOWN = 2 + +uint32 XBOX360_PAD_LEFT = 3 + +uint32 XBOX360_PAD_RIGHT = 4 + +uint32 XBOX360_FILE_BUTTON_START = 5 + +uint32 XBOX360_DOCUMENT_BUTTON_BACK = 6 + +uint32 XBOX360_LEFT_THUMB_BUTTON = 7 + +uint32 XBOX360_RIGHT_THUMB_BUTTON = 8 + +uint32 XBOX360_LEFT_SHOULDER = 9 + +uint32 XBOX360_RIGHT_SHOULDER = 10 + +uint32 XBOX360_BUTTON_A = 13 + +uint32 XBOX360_BUTTON_B = 14 + +uint32 XBOX360_BUTTON_X = 15 + +uint32 XBOX360_BUTTON_Y = 16 diff --git a/kortex_driver/msg/ZoneShape.msg b/kortex_driver/msg/ZoneShape.msg new file mode 100644 index 00000000..98d9ed62 --- /dev/null +++ b/kortex_driver/msg/ZoneShape.msg @@ -0,0 +1,9 @@ + + +uint32 shape_type + +Point origin + +RotationMatrix orientation +float32[] dimensions +float32 envelope_thickness \ No newline at end of file diff --git a/kortex_driver/msg/non_generated/ApiOptions.msg b/kortex_driver/msg/non_generated/ApiOptions.msg new file mode 100644 index 00000000..471fddd8 --- /dev/null +++ b/kortex_driver/msg/non_generated/ApiOptions.msg @@ -0,0 +1 @@ +uint32 timeout_ms \ No newline at end of file diff --git a/kortex_driver/msg/non_generated/KortexError.msg b/kortex_driver/msg/non_generated/KortexError.msg new file mode 100644 index 00000000..45392615 --- /dev/null +++ b/kortex_driver/msg/non_generated/KortexError.msg @@ -0,0 +1,3 @@ +string description +uint32 code +uint32 subCode \ No newline at end of file diff --git a/kortex_driver/package.xml b/kortex_driver/package.xml new file mode 100644 index 00000000..3a23ad1e --- /dev/null +++ b/kortex_driver/package.xml @@ -0,0 +1,30 @@ + + + kortex_driver + 1.0.0 + THe kortex package that act as a robot's driver. + + KINOVA + + BSD + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_generation + message_runtime + + + + + + + + diff --git a/kortex_driver/protos/Base.proto b/kortex_driver/protos/Base.proto new file mode 100644 index 00000000..15df5e47 --- /dev/null +++ b/kortex_driver/protos/Base.proto @@ -0,0 +1,1766 @@ +/** + * Base related messages. + * + */ +syntax = "proto3"; + +import public "Common.proto"; +import public "Errors.proto"; + +package Kinova.Api.Base; + +/** + * Service providing Base commands + */ +service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error + + //Creates a User Profile + rpc CreateUserProfile (FullUserProfile) returns (Kinova.Api.Common.UserProfileHandle);//@RPC_ID=1 + + //Updates an existing User Profile + rpc UpdateUserProfile (UserProfile) returns (Kinova.Api.Common.Empty);//@RPC_ID=2 + + //Retrieves an existing User Profile + rpc ReadUserProfile (Kinova.Api.Common.UserProfileHandle) returns (UserProfile);//@RPC_ID=3 + + //Deletes an existing User Profile + rpc DeleteUserProfile (Kinova.Api.Common.UserProfileHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=4 + + //Retrieves all User Profiles + rpc ReadAllUserProfiles (Kinova.Api.Common.Empty) returns (UserProfileList);//@RPC_ID=5 + + //Retrieves the list of Usernames + rpc ReadAllUsers (Kinova.Api.Common.Empty) returns (UserList);//@RPC_ID=6 + + //Changes password of an existing user + rpc ChangePassword (PasswordChange) returns (Kinova.Api.Common.Empty);//@RPC_ID=7 + + //Creates a new Sequence + rpc CreateSequence (Sequence) returns (SequenceHandle);//@RPC_ID=8 + + //Updates an existing Sequence + rpc UpdateSequence (Sequence) returns (Kinova.Api.Common.Empty);//@RPC_ID=9 + + //Updates an existing Sequence + rpc ReadSequence (SequenceHandle) returns (Sequence);//@RPC_ID=10 + + //Deletes an existing sequence + rpc DeleteSequence (SequenceHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=11 + + //Retrieves all existing sequences + rpc ReadAllSequences (Kinova.Api.Common.Empty) returns (SequenceList);//@RPC_ID=12 + + //Deletes a specific task from the specified sequence + rpc DeleteSequenceTask (SequenceTaskHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=13 + + //Deletes all tasks from the specified sequence + rpc DeleteAllSequenceTasks (SequenceHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=14 + + //Plays an existing sequence + rpc PlaySequence (SequenceHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=15 + + //Plays an existing sequence with options + rpc PlayAdvancedSequence (AdvancedSequenceHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=16 + + //Stops execution of currently played sequence + rpc StopSequence (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=17 + + //Pauses execution of currently played sequence + rpc PauseSequence (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=18 + + //Resumes execution of currently paused sequence + rpc ResumeSequence (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=19 + + //Creates a Protection zone + rpc CreateProtectionZone (ProtectionZone) returns (ProtectionZoneHandle);//@RPC_ID=20 + + //Updates an existing Protection zone + rpc UpdateProtectionZone (ProtectionZone) returns (Kinova.Api.Common.Empty);//@RPC_ID=21 + + //Retrieve an existing protection zone + rpc ReadProtectionZone (ProtectionZoneHandle) returns (ProtectionZone);//@RPC_ID=22 + + //Deletes an existing protection zone + rpc DeleteProtectionZone (ProtectionZoneHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=23 + + //Retrieves all protection zones + rpc ReadAllProtectionZones (Kinova.Api.Common.Empty) returns (ProtectionZoneList);//@RPC_ID=24 + + //Creates a Mapping + rpc CreateMapping (Mapping) returns (MappingHandle);//@RPC_ID=26 + + //Retrieves an existing mapping + rpc ReadMapping (MappingHandle) returns (Mapping);//@RPC_ID=27 + + //Retrieves all mappings + rpc ReadAllMappings (Kinova.Api.Common.Empty) returns (MappingList);//@RPC_ID=30 + + //Creates a new map + rpc CreateMap (Map) returns (MapHandle);//@RPC_ID=36 + + //Retrieves all maps associated to the specified mapping + rpc ReadAllMaps (MappingHandle) returns (MapList);//@RPC_ID=40 + + //Makes the specified Map active within the specified Map group and Mapping + rpc ActivateMap (ActivateMapHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=41 + + //Creates an action + rpc CreateAction (Action) returns (ActionHandle);//@RPC_ID=42 + + //Retrieves an existing action + rpc ReadAction (ActionHandle) returns (Action);//@RPC_ID=43 + + //Retrieves all existing actions + rpc ReadAllActions (RequestedActionType) returns (ActionList);//@RPC_ID=44 + + //Deletes an existing action + rpc DeleteAction (ActionHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=45 + + //Update an existing action + rpc UpdateAction (Action) returns (Kinova.Api.Common.Empty);//@RPC_ID=46 + + //Asks the robot to executes the specified existing action + rpc ExecuteActionFromReference (ActionHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=47 + + //Asks the robot to execute the specified action + rpc ExecuteAction (Action) returns (Kinova.Api.Common.Empty);//@RPC_ID=48 + + //Pauses the execution of the currently executed action. ResumeAction can be invoked afterwards. + rpc PauseAction (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=49 + + //Stops the execution of the currently executed action. ResumeAction cannot be invoked afterwards. + rpc StopAction (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=50 + + //Resumes the execution of the currently paused action + rpc ResumeAction (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=51 + + //Retrieves the IPv4 network configuration for the specified Network adapter + rpc GetIPv4Configuration (NetworkHandle) returns (IPv4Configuration);//@RPC_ID=59 + + //Modifies the IPv4 network configuration for the specified Network adapter + rpc SetIPv4Configuration (FullIPv4Configuration) returns (Kinova.Api.Common.Empty);//@RPC_ID=60 + + //Enables (or disables) the specified communication interface + rpc SetCommunicationInterfaceEnable (CommunicationInterfaceConfiguration) returns (Kinova.Api.Common.Empty);//@RPC_ID=61 + + //Determines if the specified communication interface is enabled (or disabled) + rpc IsCommunicationInterfaceEnable (NetworkHandle) returns (CommunicationInterfaceConfiguration);//@RPC_ID=62 + + //Retrieves the list of available Wifi networks + rpc GetAvailableWifi (Kinova.Api.Common.Empty) returns (WifiInformationList);//@RPC_ID=63 + + //Retrieves information about a specific Wifi network + rpc GetWifiInformation (Ssid) returns (WifiInformation);//@RPC_ID=64 + + //Configures a specific Wifi network + rpc AddWifiConfiguration (WifiConfiguration) returns (Kinova.Api.Common.Empty);//@RPC_ID=65 + + //Deletes a specific Wifi network + rpc DeleteWifiConfiguration (Ssid) returns (Kinova.Api.Common.Empty);//@RPC_ID=66 + + //Retrieves the list of configured Wifi networks + rpc GetAllConfiguredWifis (Kinova.Api.Common.Empty) returns (WifiConfigurationList);//@RPC_ID=67 + + //Connects robot to specified Wifi network + rpc ConnectWifi (Ssid) returns (Kinova.Api.Common.Empty);//@RPC_ID=68 + + //Disconnects the robot from currently connected Wifi network + rpc DisconnectWifi (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=69 + + //Gets information about the connected Wifi network + rpc GetConnectedWifiInformation (Kinova.Api.Common.Empty) returns (WifiInformation);//@RPC_ID=70 + + //Unsubscribes client from receiving specified types of notifications + rpc Unsubscribe (Kinova.Api.Common.NotificationHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=97 @UNSUB + + //Subscribes to configuration change notifications + rpc ConfigurationChangeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=98 @PUB_SUB=ConfigurationChangeNotification + + //Subscribes to mapping information notifications + rpc MappingInfoTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=99 @PUB_SUB=MappingInfoNotification + + //Subscribes to control mode notifications + rpc ControlModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=100 @PUB_SUB=ControlModeNotification + + //Subscribes to operating mode notifications + rpc OperatingModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=101 @PUB_SUB=OperatingModeNotification + + //Subscribes to sequence information notifications + rpc SequenceInfoTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=102 @PUB_SUB=SequenceInfoNotification + + //Subscribes to protection zone notifications + rpc ProtectionZoneTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=103 @PUB_SUB=ProtectionZoneNotification + + //Subscribes to user notifications + rpc UserTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=104 @PUB_SUB=UserNotification + + //Subscribes to controller notifications + rpc ControllerTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=105 @PUB_SUB=ControllerNotification + + //Subscribes to action notifications + rpc ActionTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=106 @PUB_SUB=ActionNotification + + //Subscribes to robot event notifications + rpc RobotEventTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=107 @PUB_SUB=RobotEventNotification + + //Retrieves the forward kinematics that corresponds to specified transformation matrix + rpc GetFwdKinematics (Kinova.Api.Common.Empty) returns (TransformationMatrix);//@RPC_ID=108 + + //Reaches the specifed pose + rpc PlayCartesianTrajectory (ConstrainedPose) returns (Kinova.Api.Common.Empty);//@RPC_ID=109 + + //Reaches the specified position + rpc PlayCartesianTrajectoryPosition (ConstrainedPosition) returns (Kinova.Api.Common.Empty);//@RPC_ID=110 + + //Reaches the specified orientation + rpc PlayCartesianTrajectoryOrientation (ConstrainedOrientation) returns (Kinova.Api.Common.Empty);//@RPC_ID=111 + + //Pauses robot movement + rpc Pause (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=113 + + //Resumes robot movement + rpc Resume (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=114 + + //Retrieves the currently measured Pose, that is the position and orientation that the robot is currently in + rpc GetMeasuredCartesianPose (Kinova.Api.Common.Empty) returns (Pose);//@RPC_ID=115 + + //Retrieves the commanded cartesian pose + rpc GetCommandedCartesianPose (Kinova.Api.Common.Empty) returns (Pose);//@RPC_ID=116 + + //Retrieves the targeted cartesian pose + rpc GetTargetedCartesianPose (Kinova.Api.Common.Empty) returns (Pose);//@RPC_ID=117 + + //Sends a twist command + rpc SendTwistCommand (TwistCommand) returns (Kinova.Api.Common.Empty);//@RPC_ID=121 + + //Retrieves the currently measured Twist Command, that is the current linear and angular robot velocity + rpc GetMeasuredTwist (Kinova.Api.Common.Empty) returns (Twist);//@RPC_ID=122 + + //Retrieves the desired Twist Command + rpc GetCommandedTwist (Kinova.Api.Common.Empty) returns (Twist);//@RPC_ID=123 + + //Reaches the specified Joint angles + rpc PlayJointTrajectory (ConstrainedJointAngles) returns (Kinova.Api.Common.Empty);//@RPC_ID=124 + + //Reaches the specified Joint angle + rpc PlaySelectedJointTrajectory (ConstrainedJointAngle) returns (Kinova.Api.Common.Empty);//@RPC_ID=125 + + //Retrieves the currently measured joint angles, that is the current position of each joint + rpc GetMeasuredJointAngles (Kinova.Api.Common.Empty) returns (JointAngles);//@RPC_ID=126 + + //Retrieves the desired Joint angles Command + rpc GetCommandedJointAngles (Kinova.Api.Common.Empty) returns (JointAngles);//@RPC_ID=127 + + //Sends a joint speed command + rpc SendJointSpeedsCommmand (JointSpeeds) returns (Kinova.Api.Common.Empty);//@RPC_ID=132 + + //Sends a speed command for a specific joint + rpc SendSelectedJointSpeedCommand (JointSpeed) returns (Kinova.Api.Common.Empty);//@RPC_ID=133 + + //Retrieves the currently measured Joint speeds Command, that is the current speed of each joint + rpc GetMeasuredJointSpeeds (Kinova.Api.Common.Empty) returns (JointSpeeds);//@RPC_ID=134 + + //Retrieves the desired Joint speeds Command + rpc GetCommandedJointSpeeds (Kinova.Api.Common.Empty) returns (JointSpeeds);//@RPC_ID=135 + + //Sends a command to move the gripper + rpc SendGripperCommand (GripperCommand) returns (Kinova.Api.Common.Empty);//@RPC_ID=136 + + //Retrieves the current gripper movement, that is the current gripper position, force or speed + rpc GetMeasuredGripperMovement (GripperRequest) returns (Gripper);//@RPC_ID=137 + + //Retrieves the desired gripper Command + rpc GetCommandedGripperMovement (GripperRequest) returns (Gripper);//@RPC_ID=138 + + //Sets the robot in admittance mode + rpc SetAdmittance (Admittance) returns (Kinova.Api.Common.Empty);//@RPC_ID=139 + + //Defines the reference frame to use with Twist and Wrench commands + rpc SetTwistWrenchReferenceFrame (CartesianReferenceFrameRequest) returns (Kinova.Api.Common.Empty);//@RPC_ID=140 + + //sets a new operating mode. Only Maintenance, Update and Run mode are permitted + rpc SetOperatingMode (OperatingModeInformation) returns (Kinova.Api.Common.Empty);//@RPC_ID=141 + + //stops robot movement + rpc ApplyEmergencyStop (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=145 + + //resets robot stop. Robot is now permitted to move again + rpc ClearFaults (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=146 + + //retrieves currently active map associate to specified mapping. + rpc GetActiveMap (MappingHandle) returns (Kinova.Api.Common.Empty);//@RPC_ID=149 + + //retrieves current control mode + rpc GetControlMode (Kinova.Api.Common.Empty) returns (ControlModeInformation);//@RPC_ID=150 + + //retreieves current operating mode + rpc GetOperatingMode (Kinova.Api.Common.Empty) returns (OperatingModeInformation);//@RPC_ID=151 + + //sets the servoing mode + rpc SetServoingMode (ServoingModeInformation) returns (Kinova.Api.Common.Empty);//@RPC_ID=152 + + //retrieves current servoing mode + rpc GetServoingMode (Kinova.Api.Common.Empty) returns (ServoingModeInformation);//@RPC_ID=153 + + //Subscribes to servoing mode notifications + rpc ServoingModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=154 @PUB_SUB=ServoingModeNotification + + //retrieves the last sequence state + rpc GetSequenceState (SequenceHandle) returns (SequenceInformation);//@RPC_ID=156 + + //retrieves the last state of the robot versus the specified protection zone + rpc GetProtectionZoneState (ProtectionZoneHandle) returns (ProtectionZoneInformation);//@RPC_ID=157 + + //retrieves the action execution state + rpc GetActionExecutionState (Kinova.Api.Common.Empty) returns (ActionExecutionState);//@RPC_ID=158 + + //Deletes all configuration and put back factory content only (except Network settings) + rpc RestoreFactorySettings (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=160 + + //Puts back Network settings to their factory defaults and predefined users to default password + rpc RestoreNetworkFactorySettings (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=161 + + //allows to reboot robot + rpc Reboot (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=162 + + //Subscribes to factory notifications + rpc FactoryTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=164 @PUB_SUB=FactoryNotification + + //Retrieves the list of all connected controllers + rpc GetAllConnectedControllers (Kinova.Api.Common.Empty) returns (ControllerList);//@RPC_ID=166 + + //retrieves the state of a specific controller + rpc GetControllerState (ControllerHandle) returns (ControllerState);//@RPC_ID=167 + + //Retrieves the number of Actuators + rpc GetActuatorCount (Kinova.Api.Common.Empty) returns (ActuatorInformation);//@RPC_ID=171 + + //Initiates wifi scanning + rpc StartWifiScan (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=172 + + //Retrieves a configured Wifi network + rpc GetConfiguredWifi (Ssid) returns (WifiConfiguration);//@RPC_ID=173 + + //Subscribes to network event notifications + rpc NetworkTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=174 @PUB_SUB=NetworkNotification + + //Retrieves current arm state + rpc GetArmState (Kinova.Api.Common.Empty) returns (ArmStateInformation);//@RPC_ID=175 + + //Subscribes to arm state notifications + rpc ArmStateTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=176 @PUB_SUB=ArmStateNotification + + //Retrieves the IPv4 network information for the specified Network adapter + rpc GetIPv4Information (NetworkHandle) returns (IPv4Information);//@RPC_ID=177 +} + +enum ServiceVersion { + RESERVED_0 = 0; + CURRENT_VERSION = 1; // Current Version +} + +/** + * Message used to create a new User Profile in robot + */ +message FullUserProfile { + UserProfile user_profile = 1; //User Profile, which includes username + string password = 2; //User's password +} + +/** + * Information about a user + */ +message UserProfile { + Kinova.Api.Common.UserProfileHandle handle = 1; //User handle (no need to set it with CreateUserProfile() + string username = 2; // username, which is used to connect to robot (or login via Web App) + string firstname = 3; //user first name + string lastname = 4; //user last name + string application_data = 5; //other application data (used by Web App) +} + +/** + * List of user profiles + */ +message UserProfileList { + repeated UserProfile user_profiles = 1; //user profiles +} + +/** + * Message used to retrieve a list of users along with their username + */ +message UserList { + repeated Kinova.Api.Common.UserProfileHandle user_handles = 1; //user handles +} + +/** + * Message used to change a User's password + */ +message PasswordChange { + Kinova.Api.Common.UserProfileHandle handle = 1; // user handle + string old_password = 2; // current user's password + string new_password = 3; // new user's password +} + +/** + * Handle to an existing Sequence + */ +message SequenceHandle { + uint32 identifier = 1; //sequence identifier + fixed32 permission = 2; //must use 'Permission' enum +} + +/** + * Message used to pass options when executing a sequence + */ +message AdvancedSequenceHandle { + SequenceHandle handle = 1; // sequence handle + /** + * set to true to play the sequence in loop, false otherwise. When a sequence is executed in loop, it will automatically + * go back to first task within the sequence after completing execution of last task and continue execution forever + * unless the sequence is explicitely stopped + */ + bool in_loop = 2; +} + +/** + * Message used to delete a specific task within a sequence + */ +message SequenceTaskHandle { + SequenceHandle sequence_handle = 1; // sequence handle + uint32 task_index = 2; //specified task to delete within sequence +} + +/** + * Definition of a specific task within a sequence + */ +message SequenceTask { + uint32 group_identifier = 1; //specifies the order in which this task must be executed + Action action = 2; + string application_data = 3; //application data (used by Web App) +} + +/** + * Sequence definition + */ +message Sequence { + SequenceHandle handle = 1; //sequence handle + string name = 2; //sequence name + string application_data = 3; // application data (used by Web App) + repeated SequenceTask tasks = 4; // list of tasks that this sequence contains +} + +/** + * List of sequences + */ +message SequenceList { + repeated Sequence sequence_list = 1; //sequences +} + +//action to append an action to an existing trajectory (which can be already playing) +message AppendActionInformation { + SequenceHandle sequence_handle = 1; + Action action = 2; +} + +/** + * Action handle + */ +message ActionHandle { + uint32 identifier = 1; + ActionType action_type = 2; //allows to know the type of action that is affected (ex. on a notification) + fixed32 permission = 3; //must use 'Permission' enum +} + +message RequestedActionType { + ActionType action_type = 1; +} + +/** + * Definition of an Action + */ +message Action { + ActionHandle handle = 1; // action handle (useful when updating an existing action) + string name = 2; // action friendly name + string application_data = 3; // application data (used by Web App) + oneof action_parameters { + TwistCommand send_twist_command = 4; //action to control the robot in velocity + JointSpeeds send_joint_speeds = 7; //action to control each joint speed + ConstrainedPose reach_pose = 9; //action to reach a pose + ConstrainedJointAngles reach_joint_angles = 10; //action to reach a joint angles + AdmittanceMode toggle_admittance_mode = 16; //action to enable or disable the admittance mode + SwitchControlMapping switch_control_mapping = 19; //action to switch the active controller map + JointNavigationDirection navigate_joints = 20; //action to select the next actuator to control in a map + NavigationDirection navigate_mappings = 21; //action to select a different map + ChangeTwist change_twist = 25; //action to change twist + ChangeJointSpeeds change_joint_speeds = 26; //action to change the joint speeds per joint + EmergencyStop apply_emergency_stop = 31; //action to apply robot emergency stop + Faults clear_faults = 32; //action to clear faults. robot will be able to move is no more error safeties + Delay delay = 34; //action to apply a delay + ActionHandle execute_action = 35; //action to execute an existing action + GripperCommand send_gripper_command = 36; //action to execute a gripper movement + Stop stop_action = 38; //action to stop movement + } +} + +/** + * List of all supported action types + */ +enum ActionType { + UNSPECIFIED_ACTION = 0; //unspecified action type + SEND_TWIST_COMMAND = 1; //action to control the robot in velocity + SEND_JOINT_SPEEDS = 4; //action to control each joint speed + REACH_POSE = 6; //action to reach a pose + REACH_JOINT_ANGLES = 7; //action to reach a joint angles + TOGGLE_ADMITTANCE_MODE = 13; //action to enable or disable the admittance mode + SWITCH_CONTROL_MAPPING = 16; //action to switch the active controller map + NAVIGATE_JOINTS = 17; //action to select the next actuator to control from control mapping + NAVIGATE_MAPPINGS = 18; //action to select a different map + CHANGE_TWIST = 22; //action to change the twist + CHANGE_JOINT_SPEEDS = 23; //action to change the joint speeds per joint + APPLY_EMERGENCY_STOP = 28; //action to apply robot emergency stop + CLEAR_FAULTS = 29; //action to clear faults. robot will be able to move is no more error safeties + DELAY = 31; //action to apply a delay + EXECUTE_ACTION = 32; //action to execute an existing action + SEND_GRIPPER_COMMAND = 33; //action to execute a gripper command + STOP_ACTION = 35; // action to stop movement +} + +//action to switch the active controller map +message SwitchControlMapping { + uint32 controller_identifier = 1; + MapGroupHandle map_group_handle = 2; + MapHandle map_handle = 3; +} + +//action to change the maximum cartesian velocity +message ChangeTwist { + float linear = 1; + float angular = 2; +} + +//action to change the maximum angular velocity per joint +message ChangeJointSpeeds { + JointSpeeds joint_speeds = 1; +} + +//action to apply robot emergency stop +message EmergencyStop { +} + +//Faults message +message Faults { +} + +//action to apply a delay +message Delay { + uint32 duration = 1; +} + +//action to stop movement +message Stop { +} + +/** + * List of actions + */ +message ActionList { + repeated Action action_list = 1; +} + +/** + * Message to specify a timeout + */ +message Timeout { + uint32 value = 1; //timeout value +} + +/** + * Ssid handle + */ +message Ssid { + string identifier = 1; //SSID value +} + +/** + * Message to enable (or disable) a specific communication interface + */ +message CommunicationInterfaceConfiguration { + NetworkType type = 1; //network type + bool enable = 2; //true if network is enabled, false otherwise +} + +/** + * Message to identify the type of network + */ +enum NetworkType { + UNSPECIFIED_NETWORK_TYPE = 0; //unspecified network type + WIFI = 1; //Wifi network + WIRED_ETHERNET = 2; //wired Ethernet network +} + +/** + * Network handle + */ +message NetworkHandle { + NetworkType type = 1; //network type +} + +/** + * IPv4 configuration + */ +message IPv4Configuration { + uint32 ip_address = 1; //IPv4 address + uint32 subnet_mask = 2; //IPv4 subnet mask + uint32 default_gateway = 3; //Gateway IPv4 address + bool dhcp_enabled = 4; //set to true to use dhcp instead of static ip configuration +} + +/** + * IPv4 information + */ +message IPv4Information { + uint32 ip_address = 1; //IPv4 address + uint32 subnet_mask = 2; //IPv4 subnet mask + uint32 default_gateway = 3; //Gateway IPv4 address +} + +/** + * Message to specify the IPv4 configuration for a specific type of network + */ +message FullIPv4Configuration { + + NetworkHandle handle = 1; //network handle + IPv4Configuration ipv4_configuration = 2; //IPv4 configuration +} + + +/** + * Wifi Security type + */ +enum WifiSecurityType { + UNSPECIFIED_AUTHENTICATION = 0; //unspecified wifi security type + + WEP = 1; //WEP authentication required + WPA2_PERSONAL = 2; //WPA2 Personal authentication required + WPA_PERSONAL = 4; //WPA Personal authentication required + NO_AUTHENTICATION = 8; //no authentication required +} + +/** + * Wifi encryption type + */ +enum WifiEncryptionType { + + UNSPECIFIED_ENCRYPTION = 0; //unspecified wifi encryption type + AES_ENCRYPTION = 1; //AES encryption + TKIP_ENCRYPTION = 2; // TKIP encryption + WEP_ENCRYPTION = 4; //WEP encryption +} + +/** + * Signal quality + */ +enum SignalQuality { + + UNSPECIFIED_SIGNAL_QUALITY = 0; //unspecified signal quality + POOR = 1; //Poor signal quality + FAIR = 2; //Fair signal quality + GOOD = 3; //Good signal quality + EXCELLENT = 4; //Excellent signal quality + NONE = 5; //No signal +} + +/** + * Message to provide Information about a specific Wifi network + */ +message WifiInformation { + Ssid ssid = 1; //SSID + fixed32 security_type = 2; //Wifi Security type + fixed32 encryption_type = 3; //Wifi Encryption type + SignalQuality signal_quality = 4; //Wifi signal quality + int32 signal_strength = 5; //Wifi signal power in dBm + uint32 frequency = 6; //Wifi operating frequency (channel) in MHz + uint32 channel = 7; //Wifi operating channel +} + +/** + * Wifi information list + */ +message WifiInformationList { + repeated WifiInformation wifi_information_list = 1; //wifi informations +} + +/** + * Wifi Configuration + */ +message WifiConfiguration { + Ssid ssid = 1; // SSID + string security_key = 2; //security key to used when connecting to Wifi network + bool connect_automatically = 3; //set to true so robot automatically connects to this Wifi network at bootup, false otherwise +} + +/** + * Wifi configuration list + */ +message WifiConfigurationList { + repeated WifiConfiguration wifi_configuration_list = 1; //wifi configurations +} + +/** + * Protection zone handle + */ +message ProtectionZoneHandle { + uint32 identifier = 1; //protection zone identifier + fixed32 permission = 2; //must use 'Permission' enum +} + +/** + * Single rotation matrix row + */ +message RotationMatrixRow { + float column1 = 1; + float column2 = 2; + float column3 = 3; +} + +/** + * 3x3 rotation matrix + */ +message RotationMatrix { + RotationMatrixRow row1 = 1; //first rotation matrix row + RotationMatrixRow row2 = 2; // + RotationMatrixRow row3 = 3; +} + +/** + * Cartesian point + */ +message Point { + float x = 1; //x + float y = 2; //y + float z = 3; //z +} + +/** + * List of Protection zone shape types + */ +enum ShapeType { + UNSPECIFIED_SHAPE = 0; //unspecified shape type + CYLINDER = 1; + SPHERE = 2; + RECTANGULAR_PRISM = 3; +} + +/** + * Protection zone shape + */ +message ZoneShape { + ShapeType shape_type = 1; //shape type + Point origin = 2; //origin of the protection zone shape from reference + RotationMatrix orientation = 3; //rotation matrix to provide shape orientation + repeated float dimensions = 4; //shape size measurement. if rectangular prism: x, y and z dimensions. if cylinder: radius and height. if sphere: radius + float envelope_thickness = 5; //thickness of envelop around shape. The envelop is of same shape type as the shape at its center +} + +/** + * Protection zone configuration + */ +message ProtectionZone { + ProtectionZoneHandle handle = 1; //protection zone handle + string name = 2; //protection zone friendly name + string application_data = 3; //application data (used by Web App) + bool is_enabled = 4; //true if protection zone is enabled, false otherwise + ZoneShape shape = 5; //protection zone shape + repeated CartesianLimitation limitations = 6; //list of cartesian limitation (i.e. force) + repeated CartesianLimitation envelope_limitations = 7; //list of cartesian limitation of the envelop (i.e. velocity and force) +} + +/** + * list of protection zones + */ +message ProtectionZoneList { + repeated ProtectionZone protection_zones = 1; //protection zones +} + +/** + * List of limitation types + */ +enum LimitationType { + UNSPECIFIED_LIMITATION = 0; //unspecified limitation + FORCE_LIMITATION = 1; //force limitation + ACCELERATION_LIMITATION = 2; //acceleration limitation + VELOCITY_LIMITATION = 3; //velocity limitation +} + + +/** + * Limitation identifier + */ +message LimitationTypeIdentifier { + LimitationType type = 1; //limitation type +} + +/** + * Cartesian limitation configuration + */ +message CartesianLimitation { + LimitationType type = 1; //limitation type + float translation = 2; //translation limitation + float orientation = 3; //orientation limitation +} + +/** + * List of cartesian limitations + */ +message CartesianLimitationList { + repeated CartesianLimitation limitations = 1; //limitations +} + +/** + * Joint limitation + */ +message JointLimitationValue { + LimitationType type = 1; //limitation type + float value = 2; //limitation value +} + +/** + * List of joint limitations + */ +message JointLimitationValueList { + repeated JointLimitationValue joint_limitation_values = 1; //joint limitations +} + +/** + * Joint limitation + */ +message JointLimitation { + uint32 device_identifier = 1; //joint device identifier + JointLimitationValue limitation_value = 2; //joint limitation value +} + +/** + * message to specify a specific limitation type for a specific joint + */ +message JointLimitationTypeIdentifier { + uint32 device_identifier = 1; //joint device identifier + LimitationType type = 2; //joint limitation type +} + +/** + * Message to specify the parameters of a a event log query + */ +message Query { + Kinova.Api.Common.Timestamp start_timestamp = 1; //start timestamp (set to zero to specify it) + Kinova.Api.Common.Timestamp end_timestamp = 2; //end timestamp (set to zero to not specify it) + string username = 3; //queried username (set to "" to not specify it) +} + +/** + * Configuration events + */ +enum ConfigurationNotificationEvent { + + UNSPECIFIED_CONFIGURATION_EVENT = 0; //unspecified configuration event + UPDATE = 1; //delete configuration + DELETE = 2; //update configuration + DELETE_ALL = 3; //delete all configuration + CREATE = 4; //create configuration +} + +/** + * Message that contains a Configuration change event + */ +message ConfigurationChangeNotification { + ConfigurationNotificationEvent event = 1; //configuration event + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the configuration event + oneof configuration_change { + SequenceHandle sequence_handle = 4; + ActionHandle action_handle = 5; + MappingHandle mapping_handle = 6; + MapGroupHandle map_group_handle = 7; + MapHandle map_handle = 8; + Kinova.Api.Common.UserProfileHandle user_profile_handle = 9; + ProtectionZoneHandle protection_zone_handle = 10; + Kinova.Api.Common.SafetyHandle safety_handle = 11; + NetworkHandle network_handle = 12; + Ssid ssid = 14; + } + Kinova.Api.Common.Connection connection = 15; // connection that caused the configuration event +} + +/** + * Message that contains a Mapping information event + */ +message MappingInfoNotification { + uint32 controller_identifier = 1; //identifier of the controller + MapHandle active_map_handle = 2; + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the mapping information event + Kinova.Api.Common.Connection connection = 5; // connection that caused the mapping information event +} + +/** + * list of possible robot control modes + */ +enum ControlMode { + UNSPECIFIED_CONTROL_MODE = 0; //unspecified control mode + ANGULAR_JOYSTICK = 1; //angular joystick mode + CARTESIAN_JOYSTICK = 2; //cartesian joystick mode + VISION_JOYSTICK = 3; //vision joystick mode + ANGULAR_TRAJECTORY = 4; //angular trajectory mode + CARTESIAN_TRAJECTORY = 5; //cartesian trajectory mode + CARTESIAN_ADMITTANCE = 6; //cartesian admittance mode + JOINT_ADMITTANCE = 7; //joint admittance mode + NULL_SPACE_ADMITTANCE = 8; //null space mode + IDLE = 13; //Idle +}; + +message ControlModeInformation { + ControlMode mode = 1; +} + +/** + * Message that contains a Control mode event + */ +message ControlModeNotification { + ControlMode control_mode = 1; //new control mode + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the control mode event + Kinova.Api.Common.Connection connection = 4; // connection that caused the control mode event +} + +/** + * list of robot operating modes which are used to manage robot firmware upgrade + */ +enum OperatingMode { + UNSPECIFIED_OPERATING_MODE = 0; //unspecified operating mode + MAINTENANCE_MODE = 1; //robot in maintenance mode + UPDATE_MODE = 2; //robot waiting for upgrade package + UPDATE_COMPLETED_MODE = 3; //robot update successfully completed + UPDATE_FAILED_MODE = 4; //robot update failed + SHUTTING_DOWN_MODE = 5; //robot about to shutdown + RUN_MODE = 6; //robot properly running (or normal operation mode) + UPDATING_DEVICE_MODE = 7; //robot updating device +} + +// enum used to configure the servoing mode +enum ServoingMode { + UNSPECIFIED_SERVOING_MODE = 0; //unspecified servoing mode + MULTI_LEVEL_SERVOING = 1; //multi-level servoing + SINGLE_LEVEL_SERVOING = 2; //single-level servoing + LOW_LEVEL_SERVOING = 3; //low-level servoing + BYPASS_SERVOING = 4; //bypass mode +} + +message ServoingModeInformation { + ServoingMode servoing_mode = 1; +} + +message OperatingModeInformation { + OperatingMode operating_mode = 1; //operating mode + Kinova.Api.Common.DeviceHandle device_handle = 2; //device matching operating mode (if applicable) +} + +/** + * Message that contains Operating mode event + */ +message OperatingModeNotification { + OperatingMode operating_mode = 1; //new operating mode + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the Operating mode event + Kinova.Api.Common.Connection connection = 4; // connection that caused the Operating mode event + Kinova.Api.Common.DeviceHandle device_handle = 5; //device matching operating mode (if applicable) +} + +/** + * Message that contains Servoing mode event + */ +message ServoingModeNotification { + ServoingMode servoing_mode = 1; //new operating mode + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the Operating mode event + Kinova.Api.Common.Connection connection = 4; // connection that caused the Servoing mode event +} + +/** + * list of possible sequence event types + */ +enum EventIdSequenceInfoNotification { + + UNSPECIFIED_SEQUENCE_EVENT = 0; //unspecified sequence event + END = 1; //sequence end reached + ABORT = 2; //sequence aborted + PAUSE = 3; //sequence paused + TASK_START = 4; //sequence task started + TASK_END = 5; //sequence task ended + START = 6; //sequence started +} + +/** + * Message that contains Sequence information event + */ +message SequenceInfoNotification { + EventIdSequenceInfoNotification event_identifier = 1; //sequence event type + SequenceHandle sequence_handle = 2; + uint32 task_index = 3; //task index + uint32 group_identifier = 4; //specifies the order in which this task must be executed + Kinova.Api.Common.Timestamp timestamp = 5; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 6; //user that caused the sequence event + SubErrorCodes abort_details = 7; //details if event_identifier is equal to ABORT + Kinova.Api.Common.Connection connection = 8; // connection that caused the sequence event +} + + +message SequenceInformation { + EventIdSequenceInfoNotification event_identifier = 1; //sequence event type + uint32 task_index = 2; //task index + uint32 task_identifier = 3; //task identifier +} + +/** + * list of protection zone event types + */ +enum ProtectionZoneEvent { + UNSPECIFIED_PROTECTION_ZONE_EVENT = 0; + + REACHED = 1; //protection zone limit is reached + ENTERED = 2; //protection zone limit is entered + EXITED = 3; //protection zone limit is exited +}; + +/** + * Message that contains a protection zone event + */ +message ProtectionZoneNotification { + ProtectionZoneEvent event = 1; //event type + ProtectionZoneHandle handle = 2; + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the protection zone event to occur + Kinova.Api.Common.Connection connection = 5; // connection that caused the protection zone event to occur +} + +message ProtectionZoneInformation { + ProtectionZoneEvent event = 1; //event type +} + +/** + * List of User event types + */ +enum UserEvent { + UNSPECIFIED_USER_EVENT = 0; //unspecified user event + LOGGED_OUT = 1; //user logged out + LOGGED_IN = 2; //user logged in +}; + +/** + * Message that contains a User event + */ +message UserNotification { + UserEvent user_event = 1; //user event type + Kinova.Api.Common.UserProfileHandle modified_user = 2; //user profile that was modified + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the user profile event (i.e. user who changed the user profile) + Kinova.Api.Common.Connection connection = 5; // connection that caused the user profile event (i.e. user who changed the user profile) +} + +/** + * List of possible controller types + */ +enum ControllerType { + UNSPECIFIED_CONTROLLER_TYPE = 0; //unspecified controller type + XBOX_CONTROLLER = 1; //XBOX controller + WRIST_CONTROLLER = 2; //Wrist controller on Kinova Arm +} + +/** + * Handle on a specific controller + */ +message ControllerHandle { + ControllerType type = 1; //controller type + uint32 controller_identifier = 2; //controller identifier +} + +/** + * Handle on a specific button (or axis) of a controller + */ +message ControllerElementHandle { + ControllerHandle controller_handle = 1; //controller handle + oneof identifier { + uint32 button = 2; //button identifier (only set if 'button' controller event, otherwise zero) + uint32 axis = 3; //axis identifier (only set if 'axis' controller event, otherwise zero) + } +} + +/** + * Message that contains controller event + */ +message ControllerNotification { + + oneof state { + ControllerState controller_state = 1; + ControllerElementState controller_element = 2; + } + + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the controller event + Kinova.Api.Common.Connection connection = 5; // connection that caused the controller event +} + +/** + * Message that allows to retrieve a list of controller handles + */ +message ControllerList { + repeated ControllerHandle handles = 1; +} + +message ControllerState { + ControllerHandle handle = 1; + ControllerEventType event_type = 2; +} + +message ControllerElementState { + ControllerElementHandle handle = 1; //controller element + ControllerElementEventType event_type = 2; + float axis_value = 3; //axis value (set between -1.0 and +1.0); only set if 'axis' controller element, otherwise zero +} + +/** + * Controller event type + */ +enum ControllerEventType { + UNSPECIFIED_CONTROLLER_EVENT = 0; //unspecified controller event + CONTROLLER_DISCONNECTED = 1; //controller was disconnected + CONTROLLER_CONNECTED = 2; //controler was connected +} + + +/* + * Controller element event type + */ +enum ControllerElementEventType { + UNSPECIFIED_CONTROLLER_ELEMENT_EVENT = 0; //unspecified controller element event + AXIS_MOVED = 1; //controller axis moved + BUTTON_DOWN = 2; //controller button pressed + BUTTON_UP = 3; //controller button released +} + + + +/** + * Action event type + */ +enum ActionEvent { + UNSPECIFIED_ACTION_EVENT = 0; //unspecified action event + ACTION_END = 1; //Action execution end reached + ACTION_ABORT = 2; //Action execution aborted + ACTION_PAUSE = 3; //Action execution paused + ACTION_START = 4; //Action execution started +} + +/** + * Message that contains an Action event + */ +message ActionNotification { + ActionEvent action_event = 1; //Action event type + ActionHandle handle = 2; + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the action event + SubErrorCodes abort_details = 5; //details if action_event is equal to ACTION_ABORT + Kinova.Api.Common.Connection connection = 6; // connection that caused the action event + +} + +message ActionExecutionState { + ActionEvent action_event = 1; //Action event type + ActionHandle handle = 2; +} + +/** + * list of robot events + */ +enum RobotEvent { + + UNSPECIFIED_ROBOT_EVENT = 0; //unspecified robot event + ARM_CONNECTED = 1; //arm connected + ARM_DISCONNECTED = 2; //arm disconnected + TOOL_CONNECTED = 5; //tool connected to the end effector + TOOL_DISCONNECTED = 6; //tool disconnected from end effector +} + +/** + * Message that contains robot event + */ +message RobotEventNotification { + RobotEvent event = 1; //robot event type + Kinova.Api.Common.DeviceHandle handle = 2; //identifier of the hardware device connected or disconnected + Kinova.Api.Common.Timestamp timestamp = 3; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 4; //user that caused the robot event to occur + Kinova.Api.Common.Connection connection = 6; // connection that caused the robot event to occur +} + +/** + * list of backup events + */ +enum BackupEvent { + UNSPECIFIED_BACKUP_EVENT = 0; //unspecified backup event + BACKUP_RESTORED = 1; //configuration backup restored + BACKUP_UPLOADED = 2; //configuration backup uploaded on robot +} + +/** + * list of factory events + */ +enum FactoryEvent { + UNSPECIFIED_FACTORY_EVENT = 0; //unspecified factory event + FACTORY_DEFAULT_RESTORED = 1; // factory defaults restored on robot + NETWORK_FACTORY_DEFAULT_RESTORED = 2; //network factory defaults restored on robot +} + +/** + * Message that contains a factory event + */ +message FactoryNotification { + FactoryEvent event = 1; + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the factory event to occur + Kinova.Api.Common.Connection connection = 4; // connection that caused the factory event to occur +} + +/** + * list of network events + */ +enum NetworkEvent { + UNSPECIFIED_NETWORK_EVENT = 0; //unspecified network event + WIFI_CONNECTED = 1; //WiFi connected + WIFI_DISCONNECTED = 2; //Wifi Disconnected + WIFI_SCAN_STARTED = 3; //Wifi scan was initiated + WIFI_SCAN_RESULTS = 4; //Wifi scan results are available + WIFI_SCAN_FAILED = 5; //Wifi scan failed + WIFI_NOT_FOUND = 6; //Wifi selected network not found + WIFI_ASSOC_REJECTED = 7; //Wifi AP rejected association + WIFI_AUTH_WRONG_KEY = 8; //Wifi wrong PSK supplied + WIFI_AUTH_CONN_FAILED = 9; //Wifi connection failure during auth + WIFI_AUTH_FAILED = 10; //Wifi authentication failure +} + +/** + * Message that contains a network event + */ +message NetworkNotification { + NetworkEvent event = 1; + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; //user that caused the network event to occur + Kinova.Api.Common.Connection connection = 4; // connection that caused the network event to occur +} + +/** + * list of configuration change notifications + */ +message ConfigurationChangeNotificationList { + repeated ConfigurationChangeNotification notifications = 1; //notifications +} + +/** + * list of Mapping information notifications + */ +message MappingInfoNotificationList { + repeated MappingInfoNotification notifications = 1; //notifications +} + +/** + * list of Control mode notifications + */ +message ControlModeNotificationList { + repeated ControlModeNotification notifications = 1; //notifications +} + +/** + * list of Operating mode notifications + */ +message OperatingModeNotificationList { + repeated OperatingModeNotification notifications = 1; //notifications +} + +/** + * list of Servoing mode notifications + */ +message ServoingModeNotificationList { + repeated ServoingModeNotification notifications = 1; //notifications +} + +/** + * list of Sequence information notifications + */ +message SequenceInfoNotificationList { + repeated SequenceInfoNotification notifications = 1; //notifications +} + +/** + * list of Protection zone notifications + */ +message ProtectionZoneNotificationList { + repeated ProtectionZoneNotification notifications = 1; //notifications +} + +/** + * list of User notifications + */ +message UserNotificationList { + repeated UserNotification notifications = 1; //notifications +} + +/** + * list of safety notifications + */ +message SafetyNotificationList { + repeated Kinova.Api.Common.SafetyNotification notifications = 1; //notifications +} + +/** + * list of controller notifications + */ +message ControllerNotificationList { + repeated ControllerNotification notifications = 1; //notifications +} + +/** + * list of Action notifications + */ +message ActionNotificationList { + repeated ActionNotification notifications = 1; //notifications +} + +/** + * list of Robot event notifications + */ +message RobotEventNotificationList { + repeated RobotEventNotification notifications = 1; //notifications +} + +/** + * list of Network event notifications + */ +message NetworkNotificationList { + repeated NetworkNotification notifications = 1; //notifications +} + +/** + * Mapping handle + */ +message MappingHandle { + uint32 identifier = 1; //mapping identifier + fixed32 permission = 2; //must use 'Permission' enum +} + +/** + * list of possible controller input types + */ +enum ControllerInputType { + UNSPECIFIED_CONTROLLER_INPUT_TYPE = 0; + ANALOG = 1; //axis + DIGITAL = 2; //button +} + +/** + * List of possible controller input behaviors + */ +enum ControllerBehavior { + UNSPECIFIED_CONTROLLER_BEHAVIOR = 0; + CONTROLLER_BUTTON_DOWN = 1; //pushing button down + CONTROLLER_BUTTON_UP = 2; //releasing button + CONTROLLER_AXIS_POSITIVE = 3; //positive axis movement + CONTROLLER_AXIS_NEGATIVE = 4; //negative axis movement + CONTROLLER_BUTTON_CLICK = 5; //button down and up within X msec +} + +message SafetyEvent { + Kinova.Api.Common.SafetyHandle safety_handle = 1; +} + +message ControllerEvent { + ControllerInputType input_type = 1; + ControllerBehavior behavior = 2; + uint32 input_identifier = 3; +} + +message GpioEvent { + GpioState gpio_state = 1; + uint32 device_identifier = 2; +} + +/** + * List of Map events + */ +message MapEvent { + oneof events { + SafetyEvent safety_event = 1; + GpioEvent gpio_event = 2; + ControllerEvent controller_event = 3; + } + string name = 4;//event friendly name +} + +/** + * message to associate an event to an action + */ +message MapElement { + MapEvent event = 1; + Action action = 2; +} + +message ActivateMapHandle { + MappingHandle mapping_handle = 1; + MapGroupHandle map_group_handle = 2; + MapHandle map_handle = 3; +} + +message Map { + MapHandle handle = 1; //not set on createMap call + string name = 2; + repeated MapElement elements = 3; +} + +message MapHandle { + uint32 identifier = 1; + fixed32 permission = 2; //must use 'Permission' enum +} + +message MapList { + repeated Map map_list = 1; +} + +message MapGroupHandle { + uint32 identifier = 1; + fixed32 permission = 2; //must use 'Permission' enum +} + +message MapGroup { + MapGroupHandle group_handle = 1; //not set on createMapGroup call + string name = 2; + MappingHandle related_mapping_handle = 3; + MapGroupHandle parent_group_handle = 4; + repeated MapGroupHandle children_map_group_handles = 5; + repeated MapHandle map_handles = 6; + string application_data = 7; +} + +message MapGroupList { + repeated MapGroup map_groups = 1; +} + +message Mapping { + MappingHandle handle = 1; //not set on createMapping call + string name = 2; + uint32 controller_identifier = 3; + MapGroupHandle active_map_group_handle = 4; + repeated MapGroupHandle map_group_handles = 5; + MapHandle active_map_handle = 6; + repeated MapHandle map_handles = 7; + string application_data = 8; +} + +message MappingList { + repeated Mapping mappings = 1; +} + +enum SafetyIdentifier { + UNSPECIFIED_BASE_SAFETY_IDENTIFIER = 0; //0x0 + FIRMWARE_UPDATE_FAILURE = 1; //0x1 + EXTERNAL_COMMUNICATION_ERROR = 2; //0x2 + MAXIMUM_AMBIENT_TEMPERATURE = 4; //0x4 + MAXIMUM_CORE_TEMPERATURE = 8; //0x8 + JOINT_FAULT = 16; //0x10 + CYCLIC_DATA_JITTER = 32; //0x20 + REACHED_MAXIMUM_EVENT_LOGS = 64; //0x40 + NO_KINEMATICS_SUPPORT = 128; //0x80 + ABOVE_MAXIMUM_DOF = 256; //0x100 + NETWORK_ERROR = 512; //0x200 + UNABLE_TO_REACH_POSE = 1024; //0x400 + JOINT_DETECTION_ERROR = 2048; //0x800 + NETWORK_INITIALIZATION_ERROR = 4096; //0x1000 + MAXIMUM_CURRENT = 8192; //0x2000 + MAXIMUM_VOLTAGE = 16384; //0x4000 + MINIMUM_VOLTAGE = 32768; //0x8000 + MAXIMUM_END_EFFECTOR_TRANSLATION_VELOCITY = 65536; //0x10000 + MAXIMUM_END_EFFECTOR_ORIENTATION_VELOCITY = 131072; //0x20000 + MAXIMUM_END_EFFECTOR_TRANSLATION_ACCELERATION = 262144; //0x40000 + MAXIMUM_END_EFFECTOR_ORIENTATION_ACCELERATION = 524288; //0x80000 + MAXIMUM_END_EFFECTOR_TRANSLATION_FORCE = 1048576; //0x100000 + MAXIMUM_END_EFFECTOR_ORIENTATION_FORCE = 2097152; //0x200000 + MAXIMUM_END_EFFECTOR_PAYLOAD = 4194304; //0x400000 + EMERGENCY_STOP_ACTIVATED = 8388608; //0x800000 + EMERGENCY_LINE_ACTIVATED = 16777216; //0x1000000 + INRUSH_CURRENT_LIMITER_FAULT = 33554432; //0x2000000 + NVRAM_CORRUPTED = 67108864; //0x4000000 + INCOMPATIBLE_FIRMWARE_VERSION = 134217728; //0x8000000 + POWERON_SELF_TEST_FAILURE = 268435456; //0x10000000 +} + +message TransformationMatrix { + TransformationRow r0 = 1; + TransformationRow r1 = 2; + TransformationRow r2 = 3; + TransformationRow r3 = 4; +} + +message TransformationRow { + float c0 = 1; + float c1 = 2; + float c2 = 3; + float c3 = 4; +} + +message Pose { + float x = 1; + float y = 2; + float z = 3; + float theta_x = 4; + float theta_y = 5; + float theta_z = 6; +} + +message Position { + float x = 1; + float y = 2; + float z = 3; +} + +message Orientation { + float theta_x = 1; + float theta_y = 2; + float theta_z = 3; +} + +enum JointTrajectoryConstraintType { + UNSPECIFIED_JOINT_CONSTRAINT = 0; + JOINT_CONSTRAINT_DURATION = 1; + JOINT_CONSTRAINT_SPEED = 2; +} + +message CartesianSpeed { + float translation = 1; + float orientation = 2; +} + +message CartesianTrajectoryConstraint { + oneof type { + CartesianSpeed speed = 1; + float duration = 2; + } +} + +message JointTrajectoryConstraint { + JointTrajectoryConstraintType type = 1; + float value = 2; +} + +enum TwistMode { + UNSPECIFIED_TWIST_MODE = 0; + TWIST_TOOL_JOYSTICK = 1; + TWIST_BASE_FRAME_JOYSTICK = 2; + TWIST_CARTESIAN_JOYSTICK = 3; +} + +message Twist { + float linear_x = 1; + float linear_y = 2; + float linear_z = 3; + float angular_x = 4; + float angular_y = 5; + float angular_z = 6; +} + +message Admittance { + AdmittanceMode admittance_mode = 1; +} + +enum AdmittanceMode { + UNSPECIFIED_ADMITTANCE_MODE = 0; + CARTESIAN = 1; + JOINT = 2; + NULL_SPACE = 3; + DISABLED = 4; +} + +message CartesianReferenceFrameRequest { + CartesianReferenceFrame reference_frame = 1; +} + +enum CartesianReferenceFrame { + UNSPECIFIED_CARTESIAN_REFERENCE_FRAME = 0; + MIXED = 1; // translation = base, orientation = tool + TOOL = 2; // translation = tool, orientation = tool +} + +message ConstrainedPose { + Pose target_pose = 1; + CartesianTrajectoryConstraint constraint = 2; +} + +message ConstrainedPosition { + Position target_position = 1; + CartesianTrajectoryConstraint constraint = 2; +} + +message ConstrainedOrientation { + Orientation target_orientation = 1; + CartesianTrajectoryConstraint constraint = 2; +} + +message TwistCommand { + TwistMode mode = 1; + Twist twist = 2; + uint32 duration = 3; //if not 0, allows to set a limit in seconds to the TwistCommand +} + +message ConstrainedJointAngles { + JointAngles joint_angles = 1; + JointTrajectoryConstraint constraint = 2; +} + +message ConstrainedJointAngle { + uint32 joint_identifier = 1; + float value = 2; + JointTrajectoryConstraint constraint = 3; +} + +message JointAngles { + repeated JointAngle joint_angles = 1; +} + +message JointAngle { + uint32 joint_identifier = 1; + float value = 2; +} + +message JointSpeeds { + repeated JointSpeed joint_speeds = 1; + uint32 duration = 2; //if not 0, allows to set a limit in seconds to the JointsSpeed +} + +message JointSpeed { + uint32 joint_identifier = 1; + float value = 2; + uint32 duration = 3; //if not 0, allows to set a limit in seconds to the JointsSpeed +} + +enum GripperMode { + UNSPECIFIED_GRIPPER_MODE = 0; + GRIPPER_FORCE = 1; + GRIPPER_SPEED = 2; + GRIPPER_POSITION = 3; +} + +message GripperCommand { + GripperMode mode = 1; + //in position, admissible values for each finger is between 0...+1.0, where 0 is closed ans 1.0 is fully open + //in speed or torque, admissible values for each finger is between -1.0 and +1.0, where +1.0 corresponds + //to maximum opening speed and -1.0 corresponds to maximum closing speed + Gripper gripper = 2; + uint32 duration = 3;//if not 0, allows to set a limit in seconds to the GripperCommand +} + +message GripperRequest { + GripperMode mode = 1; +} + +message Gripper { + repeated Finger finger = 1; +} + +message Finger { + uint32 finger_identifier = 1; + float value = 2; +} + +enum NavigationDirection { + UNSPECIFIED_NAVIGATION_DIRECTION = 0; + NEXT = 1; + UP = 2; + DOWN = 3; + PREVIOUS = 4; +} + +enum JointNavigationDirection { + UNSPECIFIED_JOINT_NAVIGATION_DIRECTION = 0; + JOINT_NEXT = 1; + JOINT_PREVIOUS = 2; +} + +enum SoundType { + UNSPECIFIED_SOUND_TYPE = 0; + BIP_SERIES = 1; + SINGLE_BIP = 2; +} + +enum LedState { + UNSPECIFIED_LED_STATE = 0; + LED_OFF = 1; + LED_PULSE = 2; + LED_ON = 3; +} + +enum GpioState { + UNSPECIFIED_GPIO_STATE = 0; + GPIO_OFF = 1; + GPIO_PULSE = 2; + GPIO_ON = 3; +} + +message SystemTime { + uint32 sec = 1; //seconds after the minute(0-59) + uint32 min = 2; //minutes after the hour (0-59) + uint32 hour = 3;//hours since mignight (0-23) + uint32 mday = 4;//day of the month (1-31) + uint32 mon = 5; //months since January (0-11) + uint32 year = 6;//years since 1900 +} + +enum Xbox360DigitalInputIdentifier { + UNSPECIFIED_XBOX360_DIGITAL = 0; + XBOX360_PAD_UP = 1; + XBOX360_PAD_DOWN = 2; + XBOX360_PAD_LEFT = 3; + XBOX360_PAD_RIGHT = 4; + XBOX360_FILE_BUTTON_START = 5; + XBOX360_DOCUMENT_BUTTON_BACK = 6; + XBOX360_LEFT_THUMB_BUTTON = 7; + XBOX360_RIGHT_THUMB_BUTTON = 8; + XBOX360_LEFT_SHOULDER = 9; + XBOX360_RIGHT_SHOULDER = 10; + XBOX360_BUTTON_A = 13; + XBOX360_BUTTON_B = 14; + XBOX360_BUTTON_X = 15; + XBOX360_BUTTON_Y = 16; +} + +enum Xbox360AnalogInputIdentifier { + UNSPECIFIED_XBOX360_ANALOG = 0; + XBOX360_THUMB_LEFT_X = 1; + XBOX360_THUMB_LEFT_Y = 2; + XBOX360_THUMB_RIGHT_X = 3; + XBOX360_THUMB_RIGHT_Y = 4; + XBOX360_TRIGGER_LEFT = 5; + XBOX360_TRIGGER_RIGHT = 6; +} + +message ActuatorInformation { + uint32 count = 1; +} + +message ArmStateInformation { + Kinova.Api.Common.ArmState active_state = 1; + Kinova.Api.Common.Connection connection = 2; //connection information of the last processed command which triggered an arm state change +} + +/** + * Message that contains a arm state event + */ + message ArmStateNotification { + Kinova.Api.Common.ArmState active_state = 1; //new arm state + Kinova.Api.Common.Timestamp timestamp = 2; //event timestamp + Kinova.Api.Common.Connection connection = 3; //connection that caused the arm state event +} + diff --git a/kortex_driver/protos/BaseCyclic.proto b/kortex_driver/protos/BaseCyclic.proto new file mode 100644 index 00000000..31dc504c --- /dev/null +++ b/kortex_driver/protos/BaseCyclic.proto @@ -0,0 +1,195 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.BaseCyclic; + +service BaseCyclic { //@PROXY_ID=3 @ERROR=Kinova.Api.Error + rpc Refresh (Command) returns (Feedback); //@RPC_ID=1 + rpc RefreshCommand (Command) returns (Kinova.Api.Common.Empty); //@RPC_ID=2 + rpc RefreshFeedback (Kinova.Api.Common.Empty) returns (Feedback); //@RPC_ID=3 + rpc RefreshCustomData (CustomData) returns (CustomData); //@RPC_ID=4 +} + +enum ServiceVersion +{ + RESERVED_0 = 0; // Reserved + CURRENT_VERSION = 1; // Current Version +} + +//copied from ActuatorCyclic.proto +message ActuatorCommand +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 flags = 2; // Flags + float position = 3; // Desired position of the actuator in degrees + float velocity = 4; // Desired velocity of the actuator in degrees per second + float torque_joint = 5; // Desired torque of the actuator in Newton*meters + float current_motor = 6; // Desired current of the motor in Amperes +} + +//copied from InterconnectCyclic.proto +message InterconnectCommand +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 flags = 2; // Flags + float position = 3; // Desired position of the gripper fingers in percentage (0-100%) + float velocity = 4; // Desired velocity of the gripper fingers in percentage (0-100%) + float force = 5; // Desired force of the gripper fingers in percentage (0-100%) +} + +//copied from ActuatorCyclic.proto +message ActuatorFeedback +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 status_flags = 2; // Status flags + fixed32 jitter_comm = 3; // Jitter from the communication in microseconds + float position = 4; // Position of the actuator in degrees + float velocity = 5; // Velocity of the actuator in degrees per second + float torque = 6; // Torque of the actuator in Newton*meters + float current_motor = 7; // Current of the motor in Amperes + float voltage = 8; // Voltage of the main board in Volts + float temperature_motor = 9; // Motor temperature (average of the three (3) temperatures) + float temperature_core = 10; // Microcontroller temperature in degrees celsius + fixed32 fault_bank_a = 11; // Fault bank A + fixed32 fault_bank_b = 12; // Fault bank B + fixed32 warning_bank_a = 13; // Warning bank A + fixed32 warning_bank_b = 14; // Warning bank B +} + +//copied from InterconnectCyclic.proto +message InterconnectFeedback +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 status_flags = 2; // Status flags + fixed32 jitter_comm = 3; // Jitter from the communication in microseconds + float position = 4; // Position of the gripper fingers in percentage (0-100%) + float velocity = 5; // Velocity of the gripper fingers in percentage (0-100%) + float force = 6; // Force of the gripper fingers in percentage (0-100%) + float imu_acceleration_x = 7; // IMU Measured acceleration (X-Axis) of the interconnect in Meter / Second ^ squared + float imu_acceleration_y = 8; // IMU Measured acceleration (Y-Axis) of the interconnect in Meter / Second ^ squared + float imu_acceleration_z = 9; // IMU Measured acceleration (Z-Axis) of the interconnect in Meter / Second ^ squared + float imu_angular_velocity_x = 10; // IMU Measured angular velocity (X-Axis) of the interconnect in Degree / Second + float imu_angular_velocity_y = 11; // IMU Measured angular velocity (Y-Axis) of the interconnect in Degree / Second + float imu_angular_velocity_z = 12; // IMU Measured angular velocity (Z-Axis) of the interconnect in Degree / Second + float voltage = 13; // Voltage of the main board in Volt + float temperature_core = 14; // Microcontroller temperature in degree celsius + fixed32 fault_bank_a = 15; // Fault bank A + fixed32 fault_bank_b = 16; // Fault bank B + fixed32 warning_bank_a = 17; // Warning bank A + fixed32 warning_bank_b = 18; // Warning bank B +} + + +//copied from ActuatorCyclic.proto +message ActuatorCustomData +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 custom_data_0 = 2; // Custom data word 0 + fixed32 custom_data_1 = 3; // Custom data word 1 + fixed32 custom_data_2 = 4; // Custom data word 2 + fixed32 custom_data_3 = 5; // Custom data word 3 + fixed32 custom_data_4 = 6; // Custom data word 4 + fixed32 custom_data_5 = 7; // Custom data word 5 + fixed32 custom_data_6 = 8; // Custom data word 6 + fixed32 custom_data_7 = 9; // Custom data word 7 + fixed32 custom_data_8 = 10; // Custom data word 8 + fixed32 custom_data_9 = 11; // Custom data word 9 + fixed32 custom_data_10 = 12; // Custom data word 10 + fixed32 custom_data_11 = 13; // Custom data word 11 + fixed32 custom_data_12 = 14; // Custom data word 12 + fixed32 custom_data_13 = 15; // Custom data word 13 + fixed32 custom_data_14 = 16; // Custom data word 14 + fixed32 custom_data_15 = 17; // Custom data word 15 +} + +//copied from InterconnectCyclic.proto +message InterconnectCustomData +{ + fixed32 command_id = 1; // Command ID (first 2 bytes : device ID, last 2 bytes : sequence number) + fixed32 custom_data_0 = 2; // Custom data word 0 + fixed32 custom_data_1 = 3; // Custom data word 1 + fixed32 custom_data_2 = 4; // Custom data word 2 + fixed32 custom_data_3 = 5; // Custom data word 3 + fixed32 custom_data_4 = 6; // Custom data word 4 + fixed32 custom_data_5 = 7; // Custom data word 5 + fixed32 custom_data_6 = 8; // Custom data word 6 + fixed32 custom_data_7 = 9; // Custom data word 7 + fixed32 custom_data_8 = 10; // Custom data word 8 + fixed32 custom_data_9 = 11; // Custom data word 9 + fixed32 custom_data_10 = 12; // Custom data word 10 + fixed32 custom_data_11 = 13; // Custom data word 11 + fixed32 custom_data_12 = 14; // Custom data word 12 + fixed32 custom_data_13 = 15; // Custom data word 13 + fixed32 custom_data_14 = 16; // Custom data word 14 + fixed32 custom_data_15 = 17; // Custom data word 15 +} + +message BaseFeedback +{ + uint32 active_state_connection_identifier= 1; // Connection identifier of the last processed command which triggered an arm state change + Kinova.Api.Common.ArmState active_state = 2; // Active state of the arm + float arm_voltage = 3; // Arm current in Volts + float arm_current = 4; // Arm voltage in Amperes + float temperature_cpu = 5; // CPU temperature in degree Celcius + float temperature_ambient = 6; // Ambient temperature in degree Celcius + float imu_acceleration_x = 7; // IMU Measured acceleration (X-Axis) of the base in meter per second squared + float imu_acceleration_y = 8; // IMU Measured acceleration (Y-Axis) of the base in meter per second squared + float imu_acceleration_z = 9; // IMU Measured acceleration (Z-Axis) of the base in meter per second squared + float imu_angular_velocity_x = 10; // IMU Measured angular velocity (X-Axis) of the base in degree per second + float imu_angular_velocity_y = 11; // IMU Measured angular velocity (Y-Axis) of the base in degree per second + float imu_angular_velocity_z = 12; // IMU Measured angular velocity (Z-Axis) of the base in degree per second + float tool_pose_x = 13; // Measured cartesian position (X-Axis) of the End Effector (EE) in meter + float tool_pose_y = 14; // Measured cartesian position (Y-Axis) of the End Effector (EE) in meter + float tool_pose_z = 15; // Measured cartesian position (Z-Axis) of the End Effector (EE) in meter + float tool_pose_theta_x = 16; // Measured cartesian orientation (X-Axis) of the End Effector (EE) in degree + float tool_pose_theta_y = 17; // Measured cartesian orientation (Y-Axis) of the End Effector (EE) in degree + float tool_pose_theta_z = 18; // Measured cartesian orientation (Z-Axis) of the End Effector (EE) in degree + float tool_twist_linear_x = 19; // Measured cartesian linear velocity (X-Axis) of the End Effector (EE) in meter + float tool_twist_linear_y = 20; // Measured cartesian linear velocity (Y-Axis) of the End Effector (EE) in meter + float tool_twist_linear_z = 21; // Measured cartesian linear velocity (Z-Axis) of the End Effector (EE) in meter + float tool_twist_angular_x = 22; // Measured cartesian angular velocity (X-Axis) of the End Effector (EE) in degree per second + float tool_twist_angular_y = 23; // Measured cartesian angular velocity (Y-Axis) of the End Effector (EE) in degree per second + float tool_twist_angular_z = 24; // Measured cartesian angular velocity (Z-Axis) of the End Effector (EE) in degree per second + float tool_external_wrench_force_x = 25; // Calculated force in X-Axis from external wrench float in Newton + float tool_external_wrench_force_y = 26; // Calculated force in Y-Axis from external wrench float in Newton + float tool_external_wrench_force_z = 27; // Calculated force in Z-Axis from external wrench float in Newton + float tool_external_wrench_torque_x = 28; // Calculated torque about X-axis from external wrench float in Newton * Meter + float tool_external_wrench_torque_y = 29; // Calculated torque about Y-axis from external wrench float in Newton * Meter + float tool_external_wrench_torque_z = 30; // Calculated torque about Z-axis from external wrench float in Newton * Meter + fixed32 fault_bank_a = 31; // The Arm Fault flags bank A (0 if no fault) see Base.SafetyIdentifier + fixed32 fault_bank_b = 32; // The Arm Fault flags bank B (0 if no fault) see Base.SafetyIdentifier + fixed32 warning_bank_a = 33; // The Arm Warning flags bank A (0 if no warning) see Base.SafetyIdentifier + fixed32 warning_bank_b = 34; // The Arm Warning flags bank B (0 if no warning) see Base.SafetyIdentifier +} + +// Custom development data, content varies according to debug needs +message CustomData +{ + fixed32 frame_id = 1; // Frame ID + fixed32 custom_data_0 = 2; // Custom data word 0 + fixed32 custom_data_1 = 3; // Custom data word 1 + fixed32 custom_data_2 = 4; // Custom data word 2 + fixed32 custom_data_3 = 5; // Custom data word 3 + fixed32 custom_data_4 = 6; // Custom data word 4 + fixed32 custom_data_5 = 7; // Custom data word 5 + fixed32 custom_data_6 = 8; // Custom data word 6 + fixed32 custom_data_7 = 9; // Custom data word 7 + repeated ActuatorCustomData actuators_custom_data = 10; // Actuator custom data (repeated) + InterconnectCustomData interconnect_custom_data = 11; // Interconnect custom data +} + +message Command +{ + fixed32 frame_id = 1; // Frame ID + repeated ActuatorCommand actuators = 2; // Actuator command (repeated) + InterconnectCommand interconnect = 3; // Interconnect command +} + +message Feedback +{ + fixed32 frame_id = 1; // Frame ID + BaseFeedback base = 2; // Base feedback + repeated ActuatorFeedback actuators = 3; // Actuator feedback + InterconnectFeedback interconnect = 4; // Interconnect feedback +} diff --git a/kortex_driver/protos/Common.options b/kortex_driver/protos/Common.options new file mode 100644 index 00000000..5f3020ab --- /dev/null +++ b/kortex_driver/protos/Common.options @@ -0,0 +1,4 @@ +Kinova.Api.Common.DeviceTypes long_names:false +Kinova.Api.Common.SafetyStatusValue long_names:false +Kinova.Api.Common.NotificationType long_names:false +Kinova.Api.Common.Unit long_names:false \ No newline at end of file diff --git a/kortex_driver/protos/Common.proto b/kortex_driver/protos/Common.proto new file mode 100644 index 00000000..50f762ec --- /dev/null +++ b/kortex_driver/protos/Common.proto @@ -0,0 +1,152 @@ +syntax = "proto3"; + +package Kinova.Api.Common; + +/** + * list of possible device types + */ +enum DeviceTypes { + UNSPECIFIED_DEVICE_TYPE = 0; + BASE = 1; + VISION = 2; + BIG_ACTUATOR = 3; + SMALL_ACTUATOR = 4; + INTERCONNECT = 5; + GRIPPER = 6; +} + +/** +* Message contains information about a device - device type, device identifier, and the order of the device within the robot +*/ +message DeviceHandle { + DeviceTypes device_type = 1; + uint32 device_identifier = 2; // Unique device identifier (used with other services) + uint32 order = 3; // Unique value indicating the order of that device versus the others to facilitate representation +} + +/** + * list of possible safety statuses + */ +enum SafetyStatusValue { + UNSPECIFIED = 0; + WARNING = 1; //warning safety reached + ERROR = 2; //error safety reached + NORMAL = 3; //safety is off +} + +/** + * Enumeration used as bitfields wih permission field + */ +enum Permission { + NO_PERMISSION = 0; + READ_PERMISSION = 1; //refers to a user's capability to read the entity + UPDATE_PERMISSION = 2;//refers to a user's capability to write or modify the entity + DELETE_PERMISSION = 4; //refers to a user's capability to delete the entity +} + +/** + * list of notification types + */ +enum NotificationType { + UNSPECIFIED_NOTIFICATION_TYPE = 0; + THRESHOLD = 1; + FIX_RATE = 2; + EVENT = 3; //Event type. Only this one is supported for now +} + +/** + * list of units used throughout API methods + */ +enum Unit { + UNSPECIFIED_UNIT = 0; + CELSIUS = 1; + AMPERE = 2; + VOLT = 3; + METER_PER_SECOND = 4; + DEGREE_PER_SECOND = 5; + METER_PER_SECOND_2 = 6; + DEGREE_PER_SECOND_2 = 7; + NEWTON = 8; + NEWTON_METER =9; + KILOGRAM = 10; + DEGREE = 11; + TICK = 12; + DEGREE_PER_MILLISECOND = 13; +} + +/** + * Message used when no information needs to be exchanged between client application and robot, and vice versa + */ +message Empty { +} + +/** + * Notification options + */ +message NotificationOptions { + NotificationType type = 1; //type of notification + uint32 rate_m_sec = 2; + float threshold_value = 3; +} + +/** + * Handle to a safety + */ +message SafetyHandle { + uint32 identifier = 1; +} + +/** + * Handle to a notification + */ +message NotificationHandle { + uint32 identifier = 1; +} + +/** + * Message that contains a Safety event + */ +message SafetyNotification { + SafetyHandle safety_handle = 1; //safety handle + SafetyStatusValue value = 2; //new safety status + Timestamp timestamp = 3; //event timestamp + UserProfileHandle user_handle = 4; //user that caused the safety event + Connection connection = 5; // connection that caused the safety event +} + +/** + * Timestamp based on epoch + */ +message Timestamp { + uint32 sec = 1; //epoch in seconds since 1970 + uint32 usec = 2;//microseconds after the second (0-999999) +} + +/** + * Handle to an existing User Profile. + */ +message UserProfileHandle { + uint32 identifier = 1; //User profile identifier + fixed32 permission = 2; //must use 'Permission' as bitwise +} + +message Connection { + UserProfileHandle user_handle = 1; //user profile handle, or set to zero if no user logged in + string connection_information = 2; //connection info (ex. IP address with port number) + uint32 connection_identifier = 3; //connection identifier +} + +enum ArmState +{ + UNSPECIFIED_ARM_STATE = 0; + BASE_INITIALIZATION = 1; // Cannot be reported as the Base initialization must be completed before allowing user connection + IDLE = 2; + ARM_INITIALIZATION = 3; + ARM_IN_FAULT = 4; + ARM_MAINTENANCE = 5; + ARM_SERVOING_LOW_LEVEL = 6; + ARM_SERVOING_READY = 7; + ARM_SERVOING_PLAYING_SEQUENCE = 8; + ARM_SERVOING_MANUALLY_CONTROLLED = 9; + RESERVED = 255; // For debugging, this state must never be reported outside the base. this means that a state is not mapped correctly +} \ No newline at end of file diff --git a/kortex_driver/protos/Errors.options b/kortex_driver/protos/Errors.options new file mode 100644 index 00000000..6b230f88 --- /dev/null +++ b/kortex_driver/protos/Errors.options @@ -0,0 +1,2 @@ +Kinova.Api.ErrorCodes long_names:false +Kinova.Api.SubErrorCodes long_names:false diff --git a/kortex_driver/protos/Errors.proto b/kortex_driver/protos/Errors.proto new file mode 100644 index 00000000..6296884c --- /dev/null +++ b/kortex_driver/protos/Errors.proto @@ -0,0 +1,118 @@ +syntax = "proto3"; + +package Kinova.Api; + + +enum ErrorCodes { + SUCCESS = 0; //no errors + ERROR_PROTOCOL_SERVER = 1; //protocol server errors + ERROR_PROTOCOL_CLIENT = 2; //protocol client errors + ERROR_DEVICE = 3; //device errors + ERROR_INTERNAL = 4; //internal errors +} + +enum SubErrorCodes { + + SUB_SUCCESS = 0; //no sub errors + FAILED = 1; //failed method + UNIMPLEMENTED = 2; //unimplemented method + INVALID_PARAM = 3; //invalid parameter + + UNSUPPORTED_SERVICE = 4; //service not recognized + UNSUPPORTED_METHOD = 5; //method not recognized + TOO_LARGE_ENCODED_FRAME_BUFFER = 6; //encoded frame bigger than what transport permits + FRAME_ENCODING_ERR = 7; //unable to encode frame + FRAME_DECODING_ERR = 8; //unable to decode frame + INCOMPATIBLE_HEADER_VERSION = 9; //frame header version differs from what is expected and is considered incompatible + UNSUPPORTED_FRAME_TYPE = 10; //unrecognized frame type + UNREGISTERED_NOTIFICATION_RECEIVED = 11;//server receiving unregistered notification + INVALID_SESSION = 12; //session not recognized + PAYLOAD_DECODING_ERR = 13; //unable to decode payload + UNREGISTERED_FRAME_RECEIVED = 14; //client received a response for which it did not send an RPC call + + INVALID_PASSWORD = 15; //password does not match specified user + USER_NOT_FOUND = 16; //unrecognized user + + ENTITY_NOT_FOUND = 17; //cannot find entity + + ROBOT_MOVEMENT_IN_PROGRESS = 18; //robot refuses new control command because robot movement in progress + ROBOT_NOT_MOVING = 19; //robot refuses stop command because robot is not moving + + NO_MORE_STORAGE_SPACE = 20; //Unable to execute because no more storage + + ROBOT_NOT_READY = 21; //robot initialization is not complete + ROBOT_IN_FAULT = 22; //robot in fault + ROBOT_IN_MAINTENANCE = 23; //robot in maintenance + ROBOT_IN_UPDATE_MODE = 24; //robot in update + ROBOT_IN_EMERGENCY_STOP = 25; //robot in emergency stop state + + SINGLE_LEVEL_SERVOING = 26; //robot is in single level servoing mode + LOW_LEVEL_SERVOING = 27; //robot is in low level servoing mode + + MAPPING_GROUP_NON_ROOT = 28; //trying to add a non-root MapGroup to Mapping + MAPPING_INVALID_GROUP = 29; //trying to add an invalid or non-existent MapGroup to Mapping + MAPPING_INVALID_MAP = 30; //trying to add an invalid or non-existent Map to Mapping + MAP_GROUP_INVALID_MAP = 31; //trying to add an invalid or non-existent Map to MapGroup + MAP_GROUP_INVALID_PARENT = 32; //trying to add a MapGroup under an invalid parent + MAP_GROUP_INVALID_CHILD = 33; //trying to add an invalid or non-existent to MapGroup + MAP_GROUP_INVALID_MOVE = 34; //trying to change a MapGroup's parent: move not supported + MAP_IN_USE = 35; //deleting a Map used in a Mapping or MapGroup + + WIFI_CONNECT_ERROR = 36; //unable to connect to specified Wifi network + UNSUPPORTED_NETWORK_TYPE = 37; //unsupported network type + TOO_LARGE_ENCODED_PAYLOAD_BUFFER = 38; //encoded payload bigger than what transport permits + + UPDATE_PERMISSION_DENIED = 39; // attempting update command on non-updatable entity + DELETE_PERMISSION_DENIED = 40; // attempting delete command on non-deletable entity + DATABASE_ERROR = 41; // internal DB error + + UNSUPPORTED_OPTION = 42; // option not supported + UNSUPPORTED_RESOLUTION = 43; // resolution not supported + UNSUPPORTED_FRAME_RATE = 44; // frame rate not supported + UNSUPPORTED_BIT_RATE = 45; // bit rate not supported + UNSUPPORTED_ACTION = 46; // action not supported (generic, when an action is not supported for a particular item) + UNSUPPORTED_FOCUS_ACTION = 47; // focus action not supported + VALUE_IS_ABOVE_MAXIMUM = 48; // specified value is above the supported maximum + VALUE_IS_BELOW_MINIMUM = 49; // specified value is below the supported minimum + + DEVICE_DISCONNECTED = 50; // device is not connected + DEVICE_NOT_READY = 51; // device is not ready + + INVALID_DEVICE = 52; // device id is invalid during bridging + + SAFETY_THRESHOLD_REACHED = 53; // safety threshold is reached therefore safety is on + + INVALID_USER_SESSION_ACCESS = 54; // service or function access not allowed: out of session or level access + + CONTROL_MANUAL_STOP = 55; // Manually stopped sequence or action + CONTROL_OUTSIDE_WORKSPACE = 56; // Cartesian Position Commanded is outside Robot Workspace + CONTROL_ACTUATOR_COUNT_MISMATCH = 57; // Number of constraint sent does not correspond to number of actuator (ex: joint speed) + CONTROL_INVALID_DURATION = 58; // Duration constraint is too short. The robot would need out of limit speeds/accelerations to reach this duration. + CONTROL_INVALID_SPEED = 59; // Speed Constraint is negative + CONTROL_LARGE_SPEED = 60; // Speed Constraint is too high (exceed speed limit of leads to high acceleration) + CONTROL_INVALID_ACCELERATION = 61; // Speed Constraint is too high Or Duration constraint too short and leads to high acceleration + CONTROL_INVALID_TIME_STEP = 62; // Refresh rate is smaller than the duration of the trajectory + CONTROL_LARGE_SIZE = 63; // Duration of the trajectory is more than 100s. The length of the trajectory is limited to 100000 points to avoid saturating the Base memory + CONTROL_WRONG_MODE = 64; // Control Mode is not a Trajectory Mode + CONTROL_JOINT_POSITION_LIMIT = 65; // The commanded configuration contains at least one actuator which is out of its physical limits + CONTROL_NO_FILE_IN_MEMORY = 66; // Trajectory is not computed and try to be started + CONTROL_INDEX_OUT_OF_TRAJECTORY = 67; // Attempting to read a point of the trajectory with an index higher than the number of point in trajectory point list + CONTROL_ALREADY_RUNNING = 68; // Trajectory is already running + CONTROL_WRONG_STARTING_POINT = 69; // Robot is not on the first point of the trajectory when we try to start the trajectory. This can happen if there is a motion between the moment when trajectory is computed and when it is started + CONTROL_CARTESIAN_CANNOT_START = 70; // Cannot start + CONTROL_UNDEFINED_CONSTRAINT = 71; // Kontrol Library is not initialized + CONTROL_UNINITIALIZED = 72; // Contraint sent is not defined + CONTROL_NO_ACTION = 73; // Action does not exist + CONTROL_UNDEFINED = 74; // Undefined error + + WRONG_SERVOING_MODE = 75; //robot is in not in the right servoing mode + + USERNAME_LENGTH_EXCEEDED = 100; // User profile username length exceeds maximum allowed length + FIRSTNAME_LENGTH_EXCEEDED = 101; // User profile first name length exceeds maximum allowed length + LASTNAME_LENGTH_EXCEEDED = 102; // User profile last name length exceeds maximum allowed length + PASSWORD_LENGTH_EXCEEDED = 103; // User profile password length exceeds maximum allowed length + USERNAME_ALREADY_EXISTS = 104; // User profile username already in use by another profile + USERNAME_EMPTY = 105; // User profile empty username not allowed + PASSWORD_NOT_CHANGED = 106; // Change password both passwords are the same + MAXIMUM_USER_PROFILES_USED = 107; // Maximum number of user profiles in use +} diff --git a/kortex_driver/readme.md b/kortex_driver/readme.md new file mode 100644 index 00000000..14a32602 --- /dev/null +++ b/kortex_driver/readme.md @@ -0,0 +1,107 @@ + +# Kortex Driver + + + +1. [Content](#content) + 1. [build](#build) + 1. [msg](#msg) + 1. [non_generated](#non_generated) + 1. [protos](#protos) + 1. [src](#src) + 1. [srv](#srv) + 1. [non_generated](#non_generated-1) + 1. [templates](#templates) +1. [How to start the node](#how-to-start-the-node) +1. [Generation](#generation) + + + + + +## Content + +### build +This folder's only purpose is to exist as a temp folder during the generation. It should not be used. + +### msg +This folder contains every custom message used by the node **kortex\_driver**. All the .msg files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .msg files used by the node **kortex\_driver**. + +| MSG | Description | +|:---:|:---:| +| ApiOptions.msg | A set of option that is supported by the Kortex API. It is used with the service SetApiOptions. For now there is only one option called tiemout_ms and it lets the user set a timeout value on every next service call. | +| KortexError.msg | Describe the topic /KortexError. Every service call of the node kortex_driver will publish in /KortexError everytime the Kortex API returns an error. | + + +### protos +This folder contains the protobuf files from where the MSG, SRV and sources files are generated. The content of this folder should not be modified. + +### src +This folder contains all the generated source files needed to build the node. The content of this folder should not be modified. + +### srv +This folder contains every custom service used by the node **kortex\_driver**. All the .srv files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .srv files used by the node **kortex\_driver**. + +| SRV | Description | +|:---:|:---:| +| SetApiOptions.srv | It modifies the api options of the Kortex API. Once this service is called, the options set will affect every future call to the node. | +| SetDeviceID.srv | It modifies the target device (device routing feature) of the node. The default value is 0.| + + +### templates +This folder contains all the JINJA2 files needed by the protoc generator. For more details on the generation process, see the [Generation](#generation) section. + +| JINJA2 files | Description | +|:---:|:---:| +| main.jinja2 | Use to generate src/main.cpp | +| NodeServices.cpp.jinja2 | Use to generate src/node.cpp | +| NodeServices.h.jinja2 | Use to generate src/node.h | +| proto_converterCPP.jinja2 | Use to generate every src/*_proto\_converter.cpp files | +| proto_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_converterCPP.jinja2 | Use to generate every src/*_ros\_converter.cpp files | +| ros_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_enum.jinja2 | Use to generate every msg/*.msg files that represent a protobuf enum | +| ros_message.jinja2 | Use to generate every msg/*.msg files that represent a protobuf message | +| ros_oneof.jinja2 | Use to generate every msg/*.msg files that represent a protobuf oneof | +| ros_service.jinja2 | Use to generate every msg/*.msg files that represent a protobuf RPC | + + +## How to start the node + +rosrun kortex\_driver kortex\_driver 192.168.1.10 100 + +In the command above, you would be running the kortex_driver node on an Gen3 robot with an IP address of 192.168.1.10. The cyclic data would be refreshed at 100 Hz. + + +## Generation +

+The generation process is based on a custom protoc plugin. Basically, most of the generation process is in the Python file RosGeneration.py located at the package root folder. Before launching the generation make sure you have the python module Jinja2 installed on your computer. +

+ +To launch the generation of this package: + +1. Open a terminal window. +1. Browse the root of this package [YOUR\_ROS\_WORKSPACE]/src/ros\_kortex/kortex\_driver/ +1. Make sure that the file kortex_driver.sh can be executed. If not then chmod +x kortex_driver.sh +1. Run this command: protoc --plugin=protoc-gen-custom=kortex_driver.sh -I./protos/ --custom_out=./build ./protos/\*.prot +1. The result of the generation should be on thos folder: + * /src + * /msg + * /srv + diff --git a/kortex_driver/src/base_proto_converter.cpp b/kortex_driver/src/base_proto_converter.cpp new file mode 100644 index 00000000..146830e7 --- /dev/null +++ b/kortex_driver/src/base_proto_converter.cpp @@ -0,0 +1,1227 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "base_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_driver::FullUserProfile input, FullUserProfile *output) +{ + ToProtoData(input.user_profile, output->mutable_user_profile()); + output->set_password(input.password); + + return 0; +} +int ToProtoData(kortex_driver::UserProfile input, UserProfile *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_username(input.username); + output->set_firstname(input.firstname); + output->set_lastname(input.lastname); + output->set_application_data(input.application_data); + + return 0; +} +int ToProtoData(kortex_driver::UserProfileList input, UserProfileList *output) +{ + output->clear_user_profiles(); + for(int i = 0; i < input.user_profiles.size(); i++) + { + ToProtoData(input.user_profiles[i], output->add_user_profiles()); + } + + return 0; +} +int ToProtoData(kortex_driver::UserList input, UserList *output) +{ + output->clear_user_handles(); + for(int i = 0; i < input.user_handles.size(); i++) + { + ToProtoData(input.user_handles[i], output->add_user_handles()); + } + + return 0; +} +int ToProtoData(kortex_driver::PasswordChange input, PasswordChange *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_old_password(input.old_password); + output->set_new_password(input.new_password); + + return 0; +} +int ToProtoData(kortex_driver::SequenceHandle input, SequenceHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::AdvancedSequenceHandle input, AdvancedSequenceHandle *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_in_loop(input.in_loop); + + return 0; +} +int ToProtoData(kortex_driver::SequenceTaskHandle input, SequenceTaskHandle *output) +{ + ToProtoData(input.sequence_handle, output->mutable_sequence_handle()); + output->set_task_index(input.task_index); + + return 0; +} +int ToProtoData(kortex_driver::SequenceTask input, SequenceTask *output) +{ + output->set_group_identifier(input.group_identifier); + ToProtoData(input.action, output->mutable_action()); + output->set_application_data(input.application_data); + + return 0; +} +int ToProtoData(kortex_driver::Sequence input, Sequence *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_name(input.name); + output->set_application_data(input.application_data); + output->clear_tasks(); + for(int i = 0; i < input.tasks.size(); i++) + { + ToProtoData(input.tasks[i], output->add_tasks()); + } + + return 0; +} +int ToProtoData(kortex_driver::SequenceList input, SequenceList *output) +{ + output->clear_sequence_list(); + for(int i = 0; i < input.sequence_list.size(); i++) + { + ToProtoData(input.sequence_list[i], output->add_sequence_list()); + } + + return 0; +} +int ToProtoData(kortex_driver::AppendActionInformation input, AppendActionInformation *output) +{ + ToProtoData(input.sequence_handle, output->mutable_sequence_handle()); + ToProtoData(input.action, output->mutable_action()); + + return 0; +} +int ToProtoData(kortex_driver::ActionHandle input, ActionHandle *output) +{ + output->set_identifier(input.identifier); + output->set_action_type((Kinova::Api::Base::ActionType)input.action_type); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::RequestedActionType input, RequestedActionType *output) +{ + output->set_action_type((Kinova::Api::Base::ActionType)input.action_type); + + return 0; +} +int ToProtoData(kortex_driver::Action input, Action *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_name(input.name); + output->set_application_data(input.application_data); + + if(input.oneof_action_parameters.send_twist_command.size() > 0) + { + ToProtoData(input.oneof_action_parameters.send_twist_command[0], output->mutable_send_twist_command()); + } + if(input.oneof_action_parameters.send_joint_speeds.size() > 0) + { + ToProtoData(input.oneof_action_parameters.send_joint_speeds[0], output->mutable_send_joint_speeds()); + } + if(input.oneof_action_parameters.reach_pose.size() > 0) + { + ToProtoData(input.oneof_action_parameters.reach_pose[0], output->mutable_reach_pose()); + } + if(input.oneof_action_parameters.reach_joint_angles.size() > 0) + { + ToProtoData(input.oneof_action_parameters.reach_joint_angles[0], output->mutable_reach_joint_angles()); + } + if(input.oneof_action_parameters.toggle_admittance_mode.size() > 0) + { + output->set_toggle_admittance_mode((AdmittanceMode)input.oneof_action_parameters.toggle_admittance_mode[0]); + } + if(input.oneof_action_parameters.switch_control_mapping.size() > 0) + { + ToProtoData(input.oneof_action_parameters.switch_control_mapping[0], output->mutable_switch_control_mapping()); + } + if(input.oneof_action_parameters.navigate_joints.size() > 0) + { + output->set_navigate_joints((JointNavigationDirection)input.oneof_action_parameters.navigate_joints[0]); + } + if(input.oneof_action_parameters.navigate_mappings.size() > 0) + { + output->set_navigate_mappings((NavigationDirection)input.oneof_action_parameters.navigate_mappings[0]); + } + if(input.oneof_action_parameters.change_twist.size() > 0) + { + ToProtoData(input.oneof_action_parameters.change_twist[0], output->mutable_change_twist()); + } + if(input.oneof_action_parameters.change_joint_speeds.size() > 0) + { + ToProtoData(input.oneof_action_parameters.change_joint_speeds[0], output->mutable_change_joint_speeds()); + } + if(input.oneof_action_parameters.apply_emergency_stop.size() > 0) + { + ToProtoData(input.oneof_action_parameters.apply_emergency_stop[0], output->mutable_apply_emergency_stop()); + } + if(input.oneof_action_parameters.clear_faults.size() > 0) + { + ToProtoData(input.oneof_action_parameters.clear_faults[0], output->mutable_clear_faults()); + } + if(input.oneof_action_parameters.delay.size() > 0) + { + ToProtoData(input.oneof_action_parameters.delay[0], output->mutable_delay()); + } + if(input.oneof_action_parameters.execute_action.size() > 0) + { + ToProtoData(input.oneof_action_parameters.execute_action[0], output->mutable_execute_action()); + } + if(input.oneof_action_parameters.send_gripper_command.size() > 0) + { + ToProtoData(input.oneof_action_parameters.send_gripper_command[0], output->mutable_send_gripper_command()); + } + if(input.oneof_action_parameters.stop_action.size() > 0) + { + ToProtoData(input.oneof_action_parameters.stop_action[0], output->mutable_stop_action()); + } + + + return 0; +} +int ToProtoData(kortex_driver::SwitchControlMapping input, SwitchControlMapping *output) +{ + output->set_controller_identifier(input.controller_identifier); + ToProtoData(input.map_group_handle, output->mutable_map_group_handle()); + ToProtoData(input.map_handle, output->mutable_map_handle()); + + return 0; +} +int ToProtoData(kortex_driver::ChangeTwist input, ChangeTwist *output) +{ + output->set_linear(input.linear); + output->set_angular(input.angular); + + return 0; +} +int ToProtoData(kortex_driver::ChangeJointSpeeds input, ChangeJointSpeeds *output) +{ + ToProtoData(input.joint_speeds, output->mutable_joint_speeds()); + + return 0; +} +int ToProtoData(kortex_driver::EmergencyStop input, EmergencyStop *output) +{ + + return 0; +} +int ToProtoData(kortex_driver::Faults input, Faults *output) +{ + + return 0; +} +int ToProtoData(kortex_driver::Delay input, Delay *output) +{ + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::Stop input, Stop *output) +{ + + return 0; +} +int ToProtoData(kortex_driver::ActionList input, ActionList *output) +{ + output->clear_action_list(); + for(int i = 0; i < input.action_list.size(); i++) + { + ToProtoData(input.action_list[i], output->add_action_list()); + } + + return 0; +} +int ToProtoData(kortex_driver::Timeout input, Timeout *output) +{ + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_driver::Ssid input, Ssid *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_driver::CommunicationInterfaceConfiguration input, CommunicationInterfaceConfiguration *output) +{ + output->set_type((Kinova::Api::Base::NetworkType)input.type); + output->set_enable(input.enable); + + return 0; +} +int ToProtoData(kortex_driver::NetworkHandle input, NetworkHandle *output) +{ + output->set_type((Kinova::Api::Base::NetworkType)input.type); + + return 0; +} +int ToProtoData(kortex_driver::IPv4Configuration input, IPv4Configuration *output) +{ + output->set_ip_address(input.ip_address); + output->set_subnet_mask(input.subnet_mask); + output->set_default_gateway(input.default_gateway); + output->set_dhcp_enabled(input.dhcp_enabled); + + return 0; +} +int ToProtoData(kortex_driver::IPv4Information input, IPv4Information *output) +{ + output->set_ip_address(input.ip_address); + output->set_subnet_mask(input.subnet_mask); + output->set_default_gateway(input.default_gateway); + + return 0; +} +int ToProtoData(kortex_driver::FullIPv4Configuration input, FullIPv4Configuration *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + ToProtoData(input.ipv4_configuration, output->mutable_ipv4_configuration()); + + return 0; +} +int ToProtoData(kortex_driver::WifiInformation input, WifiInformation *output) +{ + ToProtoData(input.ssid, output->mutable_ssid()); + output->set_security_type(input.security_type); + output->set_encryption_type(input.encryption_type); + output->set_signal_quality((Kinova::Api::Base::SignalQuality)input.signal_quality); + output->set_signal_strength(input.signal_strength); + output->set_frequency(input.frequency); + output->set_channel(input.channel); + + return 0; +} +int ToProtoData(kortex_driver::WifiInformationList input, WifiInformationList *output) +{ + output->clear_wifi_information_list(); + for(int i = 0; i < input.wifi_information_list.size(); i++) + { + ToProtoData(input.wifi_information_list[i], output->add_wifi_information_list()); + } + + return 0; +} +int ToProtoData(kortex_driver::WifiConfiguration input, WifiConfiguration *output) +{ + ToProtoData(input.ssid, output->mutable_ssid()); + output->set_security_key(input.security_key); + output->set_connect_automatically(input.connect_automatically); + + return 0; +} +int ToProtoData(kortex_driver::WifiConfigurationList input, WifiConfigurationList *output) +{ + output->clear_wifi_configuration_list(); + for(int i = 0; i < input.wifi_configuration_list.size(); i++) + { + ToProtoData(input.wifi_configuration_list[i], output->add_wifi_configuration_list()); + } + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZoneHandle input, ProtectionZoneHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::RotationMatrixRow input, RotationMatrixRow *output) +{ + output->set_column1(input.column1); + output->set_column2(input.column2); + output->set_column3(input.column3); + + return 0; +} +int ToProtoData(kortex_driver::RotationMatrix input, RotationMatrix *output) +{ + ToProtoData(input.row1, output->mutable_row1()); + ToProtoData(input.row2, output->mutable_row2()); + ToProtoData(input.row3, output->mutable_row3()); + + return 0; +} +int ToProtoData(kortex_driver::Point input, Point *output) +{ + output->set_x(input.x); + output->set_y(input.y); + output->set_z(input.z); + + return 0; +} +int ToProtoData(kortex_driver::ZoneShape input, ZoneShape *output) +{ + output->set_shape_type((Kinova::Api::Base::ShapeType)input.shape_type); + ToProtoData(input.origin, output->mutable_origin()); + ToProtoData(input.orientation, output->mutable_orientation()); + output->clear_dimensions(); + for(int i = 0; i < input.dimensions.size(); i++) + { + output->add_dimensions(input.dimensions[i]); + } + + output->set_envelope_thickness(input.envelope_thickness); + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZone input, ProtectionZone *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_name(input.name); + output->set_application_data(input.application_data); + output->set_is_enabled(input.is_enabled); + ToProtoData(input.shape, output->mutable_shape()); + output->clear_limitations(); + for(int i = 0; i < input.limitations.size(); i++) + { + ToProtoData(input.limitations[i], output->add_limitations()); + } + output->clear_envelope_limitations(); + for(int i = 0; i < input.envelope_limitations.size(); i++) + { + ToProtoData(input.envelope_limitations[i], output->add_envelope_limitations()); + } + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZoneList input, ProtectionZoneList *output) +{ + output->clear_protection_zones(); + for(int i = 0; i < input.protection_zones.size(); i++) + { + ToProtoData(input.protection_zones[i], output->add_protection_zones()); + } + + return 0; +} +int ToProtoData(kortex_driver::LimitationTypeIdentifier input, LimitationTypeIdentifier *output) +{ + output->set_type((Kinova::Api::Base::LimitationType)input.type); + + return 0; +} +int ToProtoData(kortex_driver::CartesianLimitation input, CartesianLimitation *output) +{ + output->set_type((Kinova::Api::Base::LimitationType)input.type); + output->set_translation(input.translation); + output->set_orientation(input.orientation); + + return 0; +} +int ToProtoData(kortex_driver::CartesianLimitationList input, CartesianLimitationList *output) +{ + output->clear_limitations(); + for(int i = 0; i < input.limitations.size(); i++) + { + ToProtoData(input.limitations[i], output->add_limitations()); + } + + return 0; +} +int ToProtoData(kortex_driver::JointLimitationValue input, JointLimitationValue *output) +{ + output->set_type((Kinova::Api::Base::LimitationType)input.type); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_driver::JointLimitationValueList input, JointLimitationValueList *output) +{ + output->clear_joint_limitation_values(); + for(int i = 0; i < input.joint_limitation_values.size(); i++) + { + ToProtoData(input.joint_limitation_values[i], output->add_joint_limitation_values()); + } + + return 0; +} +int ToProtoData(kortex_driver::JointLimitation input, JointLimitation *output) +{ + output->set_device_identifier(input.device_identifier); + ToProtoData(input.limitation_value, output->mutable_limitation_value()); + + return 0; +} +int ToProtoData(kortex_driver::JointLimitationTypeIdentifier input, JointLimitationTypeIdentifier *output) +{ + output->set_device_identifier(input.device_identifier); + output->set_type((Kinova::Api::Base::LimitationType)input.type); + + return 0; +} +int ToProtoData(kortex_driver::Query input, Query *output) +{ + ToProtoData(input.start_timestamp, output->mutable_start_timestamp()); + ToProtoData(input.end_timestamp, output->mutable_end_timestamp()); + output->set_username(input.username); + + return 0; +} +int ToProtoData(kortex_driver::ConfigurationChangeNotification input, ConfigurationChangeNotification *output) +{ + output->set_event((Kinova::Api::Base::ConfigurationNotificationEvent)input.event); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::MappingInfoNotification input, MappingInfoNotification *output) +{ + output->set_controller_identifier(input.controller_identifier); + ToProtoData(input.active_map_handle, output->mutable_active_map_handle()); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ControlModeInformation input, ControlModeInformation *output) +{ + output->set_mode((Kinova::Api::Base::ControlMode)input.mode); + + return 0; +} +int ToProtoData(kortex_driver::ControlModeNotification input, ControlModeNotification *output) +{ + output->set_control_mode((Kinova::Api::Base::ControlMode)input.control_mode); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ServoingModeInformation input, ServoingModeInformation *output) +{ + output->set_servoing_mode((Kinova::Api::Base::ServoingMode)input.servoing_mode); + + return 0; +} +int ToProtoData(kortex_driver::OperatingModeInformation input, OperatingModeInformation *output) +{ + output->set_operating_mode((Kinova::Api::Base::OperatingMode)input.operating_mode); + ToProtoData(input.device_handle, output->mutable_device_handle()); + + return 0; +} +int ToProtoData(kortex_driver::OperatingModeNotification input, OperatingModeNotification *output) +{ + output->set_operating_mode((Kinova::Api::Base::OperatingMode)input.operating_mode); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + ToProtoData(input.device_handle, output->mutable_device_handle()); + + return 0; +} +int ToProtoData(kortex_driver::ServoingModeNotification input, ServoingModeNotification *output) +{ + output->set_servoing_mode((Kinova::Api::Base::ServoingMode)input.servoing_mode); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::SequenceInfoNotification input, SequenceInfoNotification *output) +{ + output->set_event_identifier((Kinova::Api::Base::EventIdSequenceInfoNotification)input.event_identifier); + ToProtoData(input.sequence_handle, output->mutable_sequence_handle()); + output->set_task_index(input.task_index); + output->set_group_identifier(input.group_identifier); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_abort_details((Kinova::Api::SubErrorCodes)input.abort_details); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::SequenceInformation input, SequenceInformation *output) +{ + output->set_event_identifier((Kinova::Api::Base::EventIdSequenceInfoNotification)input.event_identifier); + output->set_task_index(input.task_index); + output->set_task_identifier(input.task_identifier); + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZoneNotification input, ProtectionZoneNotification *output) +{ + output->set_event((Kinova::Api::Base::ProtectionZoneEvent)input.event); + ToProtoData(input.handle, output->mutable_handle()); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZoneInformation input, ProtectionZoneInformation *output) +{ + output->set_event((Kinova::Api::Base::ProtectionZoneEvent)input.event); + + return 0; +} +int ToProtoData(kortex_driver::UserNotification input, UserNotification *output) +{ + output->set_user_event((Kinova::Api::Base::UserEvent)input.user_event); + ToProtoData(input.modified_user, output->mutable_modified_user()); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ControllerHandle input, ControllerHandle *output) +{ + output->set_type((Kinova::Api::Base::ControllerType)input.type); + output->set_controller_identifier(input.controller_identifier); + + return 0; +} +int ToProtoData(kortex_driver::ControllerElementHandle input, ControllerElementHandle *output) +{ + ToProtoData(input.controller_handle, output->mutable_controller_handle()); + + if(input.oneof_identifier.button.size() > 0) + { + + output->set_button(input.oneof_identifier.button[0]); + } + if(input.oneof_identifier.axis.size() > 0) + { + + output->set_axis(input.oneof_identifier.axis[0]); + } + + + return 0; +} +int ToProtoData(kortex_driver::ControllerNotification input, ControllerNotification *output) +{ + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ControllerList input, ControllerList *output) +{ + output->clear_handles(); + for(int i = 0; i < input.handles.size(); i++) + { + ToProtoData(input.handles[i], output->add_handles()); + } + + return 0; +} +int ToProtoData(kortex_driver::ControllerState input, ControllerState *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_event_type((Kinova::Api::Base::ControllerEventType)input.event_type); + + return 0; +} +int ToProtoData(kortex_driver::ControllerElementState input, ControllerElementState *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_event_type((Kinova::Api::Base::ControllerElementEventType)input.event_type); + output->set_axis_value(input.axis_value); + + return 0; +} +int ToProtoData(kortex_driver::ActionNotification input, ActionNotification *output) +{ + output->set_action_event((Kinova::Api::Base::ActionEvent)input.action_event); + ToProtoData(input.handle, output->mutable_handle()); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_abort_details((Kinova::Api::SubErrorCodes)input.abort_details); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ActionExecutionState input, ActionExecutionState *output) +{ + output->set_action_event((Kinova::Api::Base::ActionEvent)input.action_event); + ToProtoData(input.handle, output->mutable_handle()); + + return 0; +} +int ToProtoData(kortex_driver::RobotEventNotification input, RobotEventNotification *output) +{ + output->set_event((Kinova::Api::Base::RobotEvent)input.event); + ToProtoData(input.handle, output->mutable_handle()); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::FactoryNotification input, FactoryNotification *output) +{ + output->set_event((Kinova::Api::Base::FactoryEvent)input.event); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::NetworkNotification input, NetworkNotification *output) +{ + output->set_event((Kinova::Api::Base::NetworkEvent)input.event); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ConfigurationChangeNotificationList input, ConfigurationChangeNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::MappingInfoNotificationList input, MappingInfoNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::ControlModeNotificationList input, ControlModeNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::OperatingModeNotificationList input, OperatingModeNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::ServoingModeNotificationList input, ServoingModeNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::SequenceInfoNotificationList input, SequenceInfoNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::ProtectionZoneNotificationList input, ProtectionZoneNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::UserNotificationList input, UserNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::SafetyNotificationList input, SafetyNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::ControllerNotificationList input, ControllerNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::ActionNotificationList input, ActionNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::RobotEventNotificationList input, RobotEventNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::NetworkNotificationList input, NetworkNotificationList *output) +{ + output->clear_notifications(); + for(int i = 0; i < input.notifications.size(); i++) + { + ToProtoData(input.notifications[i], output->add_notifications()); + } + + return 0; +} +int ToProtoData(kortex_driver::MappingHandle input, MappingHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::SafetyEvent input, SafetyEvent *output) +{ + ToProtoData(input.safety_handle, output->mutable_safety_handle()); + + return 0; +} +int ToProtoData(kortex_driver::ControllerEvent input, ControllerEvent *output) +{ + output->set_input_type((Kinova::Api::Base::ControllerInputType)input.input_type); + output->set_behavior((Kinova::Api::Base::ControllerBehavior)input.behavior); + output->set_input_identifier(input.input_identifier); + + return 0; +} +int ToProtoData(kortex_driver::GpioEvent input, GpioEvent *output) +{ + output->set_gpio_state((Kinova::Api::Base::GpioState)input.gpio_state); + output->set_device_identifier(input.device_identifier); + + return 0; +} +int ToProtoData(kortex_driver::MapEvent input, MapEvent *output) +{ + output->set_name(input.name); + + return 0; +} +int ToProtoData(kortex_driver::MapElement input, MapElement *output) +{ + ToProtoData(input.event, output->mutable_event()); + ToProtoData(input.action, output->mutable_action()); + + return 0; +} +int ToProtoData(kortex_driver::ActivateMapHandle input, ActivateMapHandle *output) +{ + ToProtoData(input.mapping_handle, output->mutable_mapping_handle()); + ToProtoData(input.map_group_handle, output->mutable_map_group_handle()); + ToProtoData(input.map_handle, output->mutable_map_handle()); + + return 0; +} +int ToProtoData(kortex_driver::Map input, Map *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_name(input.name); + output->clear_elements(); + for(int i = 0; i < input.elements.size(); i++) + { + ToProtoData(input.elements[i], output->add_elements()); + } + + return 0; +} +int ToProtoData(kortex_driver::MapHandle input, MapHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::MapList input, MapList *output) +{ + output->clear_map_list(); + for(int i = 0; i < input.map_list.size(); i++) + { + ToProtoData(input.map_list[i], output->add_map_list()); + } + + return 0; +} +int ToProtoData(kortex_driver::MapGroupHandle input, MapGroupHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::MapGroup input, MapGroup *output) +{ + ToProtoData(input.group_handle, output->mutable_group_handle()); + output->set_name(input.name); + ToProtoData(input.related_mapping_handle, output->mutable_related_mapping_handle()); + ToProtoData(input.parent_group_handle, output->mutable_parent_group_handle()); + output->clear_children_map_group_handles(); + for(int i = 0; i < input.children_map_group_handles.size(); i++) + { + ToProtoData(input.children_map_group_handles[i], output->add_children_map_group_handles()); + } + output->clear_map_handles(); + for(int i = 0; i < input.map_handles.size(); i++) + { + ToProtoData(input.map_handles[i], output->add_map_handles()); + } + output->set_application_data(input.application_data); + + return 0; +} +int ToProtoData(kortex_driver::MapGroupList input, MapGroupList *output) +{ + output->clear_map_groups(); + for(int i = 0; i < input.map_groups.size(); i++) + { + ToProtoData(input.map_groups[i], output->add_map_groups()); + } + + return 0; +} +int ToProtoData(kortex_driver::Mapping input, Mapping *output) +{ + ToProtoData(input.handle, output->mutable_handle()); + output->set_name(input.name); + output->set_controller_identifier(input.controller_identifier); + ToProtoData(input.active_map_group_handle, output->mutable_active_map_group_handle()); + output->clear_map_group_handles(); + for(int i = 0; i < input.map_group_handles.size(); i++) + { + ToProtoData(input.map_group_handles[i], output->add_map_group_handles()); + } + ToProtoData(input.active_map_handle, output->mutable_active_map_handle()); + output->clear_map_handles(); + for(int i = 0; i < input.map_handles.size(); i++) + { + ToProtoData(input.map_handles[i], output->add_map_handles()); + } + output->set_application_data(input.application_data); + + return 0; +} +int ToProtoData(kortex_driver::MappingList input, MappingList *output) +{ + output->clear_mappings(); + for(int i = 0; i < input.mappings.size(); i++) + { + ToProtoData(input.mappings[i], output->add_mappings()); + } + + return 0; +} +int ToProtoData(kortex_driver::TransformationMatrix input, TransformationMatrix *output) +{ + ToProtoData(input.r0, output->mutable_r0()); + ToProtoData(input.r1, output->mutable_r1()); + ToProtoData(input.r2, output->mutable_r2()); + ToProtoData(input.r3, output->mutable_r3()); + + return 0; +} +int ToProtoData(kortex_driver::TransformationRow input, TransformationRow *output) +{ + output->set_c0(input.c0); + output->set_c1(input.c1); + output->set_c2(input.c2); + output->set_c3(input.c3); + + return 0; +} +int ToProtoData(kortex_driver::Pose input, Pose *output) +{ + output->set_x(input.x); + output->set_y(input.y); + output->set_z(input.z); + output->set_theta_x(input.theta_x); + output->set_theta_y(input.theta_y); + output->set_theta_z(input.theta_z); + + return 0; +} +int ToProtoData(kortex_driver::Position input, Position *output) +{ + output->set_x(input.x); + output->set_y(input.y); + output->set_z(input.z); + + return 0; +} +int ToProtoData(kortex_driver::Orientation input, Orientation *output) +{ + output->set_theta_x(input.theta_x); + output->set_theta_y(input.theta_y); + output->set_theta_z(input.theta_z); + + return 0; +} +int ToProtoData(kortex_driver::CartesianSpeed input, CartesianSpeed *output) +{ + output->set_translation(input.translation); + output->set_orientation(input.orientation); + + return 0; +} +int ToProtoData(kortex_driver::CartesianTrajectoryConstraint input, CartesianTrajectoryConstraint *output) +{ + + if(input.oneof_type.speed.size() > 0) + { + ToProtoData(input.oneof_type.speed[0], output->mutable_speed()); + } + if(input.oneof_type.duration.size() > 0) + { + + output->set_duration(input.oneof_type.duration[0]); + } + + + return 0; +} +int ToProtoData(kortex_driver::JointTrajectoryConstraint input, JointTrajectoryConstraint *output) +{ + output->set_type((Kinova::Api::Base::JointTrajectoryConstraintType)input.type); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_driver::Twist input, Twist *output) +{ + output->set_linear_x(input.linear_x); + output->set_linear_y(input.linear_y); + output->set_linear_z(input.linear_z); + output->set_angular_x(input.angular_x); + output->set_angular_y(input.angular_y); + output->set_angular_z(input.angular_z); + + return 0; +} +int ToProtoData(kortex_driver::Admittance input, Admittance *output) +{ + output->set_admittance_mode((Kinova::Api::Base::AdmittanceMode)input.admittance_mode); + + return 0; +} +int ToProtoData(kortex_driver::CartesianReferenceFrameRequest input, CartesianReferenceFrameRequest *output) +{ + output->set_reference_frame((Kinova::Api::Base::CartesianReferenceFrame)input.reference_frame); + + return 0; +} +int ToProtoData(kortex_driver::ConstrainedPose input, ConstrainedPose *output) +{ + ToProtoData(input.target_pose, output->mutable_target_pose()); + ToProtoData(input.constraint, output->mutable_constraint()); + + return 0; +} +int ToProtoData(kortex_driver::ConstrainedPosition input, ConstrainedPosition *output) +{ + ToProtoData(input.target_position, output->mutable_target_position()); + ToProtoData(input.constraint, output->mutable_constraint()); + + return 0; +} +int ToProtoData(kortex_driver::ConstrainedOrientation input, ConstrainedOrientation *output) +{ + ToProtoData(input.target_orientation, output->mutable_target_orientation()); + ToProtoData(input.constraint, output->mutable_constraint()); + + return 0; +} +int ToProtoData(kortex_driver::TwistCommand input, TwistCommand *output) +{ + output->set_mode((Kinova::Api::Base::TwistMode)input.mode); + ToProtoData(input.twist, output->mutable_twist()); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::ConstrainedJointAngles input, ConstrainedJointAngles *output) +{ + ToProtoData(input.joint_angles, output->mutable_joint_angles()); + ToProtoData(input.constraint, output->mutable_constraint()); + + return 0; +} +int ToProtoData(kortex_driver::ConstrainedJointAngle input, ConstrainedJointAngle *output) +{ + output->set_joint_identifier(input.joint_identifier); + output->set_value(input.value); + ToProtoData(input.constraint, output->mutable_constraint()); + + return 0; +} +int ToProtoData(kortex_driver::JointAngles input, JointAngles *output) +{ + output->clear_joint_angles(); + for(int i = 0; i < input.joint_angles.size(); i++) + { + ToProtoData(input.joint_angles[i], output->add_joint_angles()); + } + + return 0; +} +int ToProtoData(kortex_driver::JointAngle input, JointAngle *output) +{ + output->set_joint_identifier(input.joint_identifier); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_driver::JointSpeeds input, JointSpeeds *output) +{ + output->clear_joint_speeds(); + for(int i = 0; i < input.joint_speeds.size(); i++) + { + ToProtoData(input.joint_speeds[i], output->add_joint_speeds()); + } + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::JointSpeed input, JointSpeed *output) +{ + output->set_joint_identifier(input.joint_identifier); + output->set_value(input.value); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::GripperCommand input, GripperCommand *output) +{ + output->set_mode((Kinova::Api::Base::GripperMode)input.mode); + ToProtoData(input.gripper, output->mutable_gripper()); + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::GripperRequest input, GripperRequest *output) +{ + output->set_mode((Kinova::Api::Base::GripperMode)input.mode); + + return 0; +} +int ToProtoData(kortex_driver::Gripper input, Gripper *output) +{ + output->clear_finger(); + for(int i = 0; i < input.finger.size(); i++) + { + ToProtoData(input.finger[i], output->add_finger()); + } + + return 0; +} +int ToProtoData(kortex_driver::Finger input, Finger *output) +{ + output->set_finger_identifier(input.finger_identifier); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_driver::SystemTime input, SystemTime *output) +{ + output->set_sec(input.sec); + output->set_min(input.min); + output->set_hour(input.hour); + output->set_mday(input.mday); + output->set_mon(input.mon); + output->set_year(input.year); + + return 0; +} +int ToProtoData(kortex_driver::ActuatorInformation input, ActuatorInformation *output) +{ + output->set_count(input.count); + + return 0; +} +int ToProtoData(kortex_driver::ArmStateInformation input, ArmStateInformation *output) +{ + output->set_active_state((Kinova::Api::Common::ArmState)input.active_state); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::ArmStateNotification input, ArmStateNotification *output) +{ + output->set_active_state((Kinova::Api::Common::ArmState)input.active_state); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} diff --git a/kortex_driver/src/base_proto_converter.h b/kortex_driver/src/base_proto_converter.h new file mode 100644 index 00000000..5c4d0e89 --- /dev/null +++ b/kortex_driver/src/base_proto_converter.h @@ -0,0 +1,308 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BasePROTO_CONVERTER_H_ +#define _KORTEX_BasePROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/FullUserProfile.h" +#include "kortex_driver/UserProfile.h" +#include "kortex_driver/UserProfileList.h" +#include "kortex_driver/UserList.h" +#include "kortex_driver/PasswordChange.h" +#include "kortex_driver/SequenceHandle.h" +#include "kortex_driver/AdvancedSequenceHandle.h" +#include "kortex_driver/SequenceTaskHandle.h" +#include "kortex_driver/SequenceTask.h" +#include "kortex_driver/Sequence.h" +#include "kortex_driver/SequenceList.h" +#include "kortex_driver/AppendActionInformation.h" +#include "kortex_driver/ActionHandle.h" +#include "kortex_driver/RequestedActionType.h" +#include "kortex_driver/Action.h" +#include "kortex_driver/SwitchControlMapping.h" +#include "kortex_driver/ChangeTwist.h" +#include "kortex_driver/ChangeJointSpeeds.h" +#include "kortex_driver/EmergencyStop.h" +#include "kortex_driver/Faults.h" +#include "kortex_driver/Delay.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/ActionList.h" +#include "kortex_driver/Timeout.h" +#include "kortex_driver/Ssid.h" +#include "kortex_driver/CommunicationInterfaceConfiguration.h" +#include "kortex_driver/NetworkHandle.h" +#include "kortex_driver/IPv4Configuration.h" +#include "kortex_driver/IPv4Information.h" +#include "kortex_driver/FullIPv4Configuration.h" +#include "kortex_driver/WifiInformation.h" +#include "kortex_driver/WifiInformationList.h" +#include "kortex_driver/WifiConfiguration.h" +#include "kortex_driver/WifiConfigurationList.h" +#include "kortex_driver/ProtectionZoneHandle.h" +#include "kortex_driver/RotationMatrixRow.h" +#include "kortex_driver/RotationMatrix.h" +#include "kortex_driver/Point.h" +#include "kortex_driver/ZoneShape.h" +#include "kortex_driver/ProtectionZone.h" +#include "kortex_driver/ProtectionZoneList.h" +#include "kortex_driver/LimitationTypeIdentifier.h" +#include "kortex_driver/CartesianLimitation.h" +#include "kortex_driver/CartesianLimitationList.h" +#include "kortex_driver/JointLimitationValue.h" +#include "kortex_driver/JointLimitationValueList.h" +#include "kortex_driver/JointLimitation.h" +#include "kortex_driver/JointLimitationTypeIdentifier.h" +#include "kortex_driver/Query.h" +#include "kortex_driver/ConfigurationChangeNotification.h" +#include "kortex_driver/MappingInfoNotification.h" +#include "kortex_driver/ControlModeInformation.h" +#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/ServoingModeInformation.h" +#include "kortex_driver/OperatingModeInformation.h" +#include "kortex_driver/OperatingModeNotification.h" +#include "kortex_driver/ServoingModeNotification.h" +#include "kortex_driver/SequenceInfoNotification.h" +#include "kortex_driver/SequenceInformation.h" +#include "kortex_driver/ProtectionZoneNotification.h" +#include "kortex_driver/ProtectionZoneInformation.h" +#include "kortex_driver/UserNotification.h" +#include "kortex_driver/ControllerHandle.h" +#include "kortex_driver/ControllerElementHandle.h" +#include "kortex_driver/ControllerNotification.h" +#include "kortex_driver/ControllerList.h" +#include "kortex_driver/ControllerState.h" +#include "kortex_driver/ControllerElementState.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionExecutionState.h" +#include "kortex_driver/RobotEventNotification.h" +#include "kortex_driver/FactoryNotification.h" +#include "kortex_driver/NetworkNotification.h" +#include "kortex_driver/ConfigurationChangeNotificationList.h" +#include "kortex_driver/MappingInfoNotificationList.h" +#include "kortex_driver/ControlModeNotificationList.h" +#include "kortex_driver/OperatingModeNotificationList.h" +#include "kortex_driver/ServoingModeNotificationList.h" +#include "kortex_driver/SequenceInfoNotificationList.h" +#include "kortex_driver/ProtectionZoneNotificationList.h" +#include "kortex_driver/UserNotificationList.h" +#include "kortex_driver/SafetyNotificationList.h" +#include "kortex_driver/ControllerNotificationList.h" +#include "kortex_driver/ActionNotificationList.h" +#include "kortex_driver/RobotEventNotificationList.h" +#include "kortex_driver/NetworkNotificationList.h" +#include "kortex_driver/MappingHandle.h" +#include "kortex_driver/SafetyEvent.h" +#include "kortex_driver/ControllerEvent.h" +#include "kortex_driver/GpioEvent.h" +#include "kortex_driver/MapEvent.h" +#include "kortex_driver/MapElement.h" +#include "kortex_driver/ActivateMapHandle.h" +#include "kortex_driver/Map.h" +#include "kortex_driver/MapHandle.h" +#include "kortex_driver/MapList.h" +#include "kortex_driver/MapGroupHandle.h" +#include "kortex_driver/MapGroup.h" +#include "kortex_driver/MapGroupList.h" +#include "kortex_driver/Mapping.h" +#include "kortex_driver/MappingList.h" +#include "kortex_driver/TransformationMatrix.h" +#include "kortex_driver/TransformationRow.h" +#include "kortex_driver/Pose.h" +#include "kortex_driver/Position.h" +#include "kortex_driver/Orientation.h" +#include "kortex_driver/CartesianSpeed.h" +#include "kortex_driver/CartesianTrajectoryConstraint.h" +#include "kortex_driver/JointTrajectoryConstraint.h" +#include "kortex_driver/Twist.h" +#include "kortex_driver/Admittance.h" +#include "kortex_driver/CartesianReferenceFrameRequest.h" +#include "kortex_driver/ConstrainedPose.h" +#include "kortex_driver/ConstrainedPosition.h" +#include "kortex_driver/ConstrainedOrientation.h" +#include "kortex_driver/TwistCommand.h" +#include "kortex_driver/ConstrainedJointAngles.h" +#include "kortex_driver/ConstrainedJointAngle.h" +#include "kortex_driver/JointAngles.h" +#include "kortex_driver/JointAngle.h" +#include "kortex_driver/JointSpeeds.h" +#include "kortex_driver/JointSpeed.h" +#include "kortex_driver/GripperCommand.h" +#include "kortex_driver/GripperRequest.h" +#include "kortex_driver/Gripper.h" +#include "kortex_driver/Finger.h" +#include "kortex_driver/SystemTime.h" +#include "kortex_driver/ActuatorInformation.h" +#include "kortex_driver/ArmStateInformation.h" +#include "kortex_driver/ArmStateNotification.h" + + +using namespace Kinova::Api::Base; + +int ToProtoData(kortex_driver::FullUserProfile intput, FullUserProfile *output); +int ToProtoData(kortex_driver::UserProfile intput, UserProfile *output); +int ToProtoData(kortex_driver::UserProfileList intput, UserProfileList *output); +int ToProtoData(kortex_driver::UserList intput, UserList *output); +int ToProtoData(kortex_driver::PasswordChange intput, PasswordChange *output); +int ToProtoData(kortex_driver::SequenceHandle intput, SequenceHandle *output); +int ToProtoData(kortex_driver::AdvancedSequenceHandle intput, AdvancedSequenceHandle *output); +int ToProtoData(kortex_driver::SequenceTaskHandle intput, SequenceTaskHandle *output); +int ToProtoData(kortex_driver::SequenceTask intput, SequenceTask *output); +int ToProtoData(kortex_driver::Sequence intput, Sequence *output); +int ToProtoData(kortex_driver::SequenceList intput, SequenceList *output); +int ToProtoData(kortex_driver::AppendActionInformation intput, AppendActionInformation *output); +int ToProtoData(kortex_driver::ActionHandle intput, ActionHandle *output); +int ToProtoData(kortex_driver::RequestedActionType intput, RequestedActionType *output); +int ToProtoData(kortex_driver::Action intput, Action *output); +int ToProtoData(kortex_driver::SwitchControlMapping intput, SwitchControlMapping *output); +int ToProtoData(kortex_driver::ChangeTwist intput, ChangeTwist *output); +int ToProtoData(kortex_driver::ChangeJointSpeeds intput, ChangeJointSpeeds *output); +int ToProtoData(kortex_driver::EmergencyStop intput, EmergencyStop *output); +int ToProtoData(kortex_driver::Faults intput, Faults *output); +int ToProtoData(kortex_driver::Delay intput, Delay *output); +int ToProtoData(kortex_driver::Stop intput, Stop *output); +int ToProtoData(kortex_driver::ActionList intput, ActionList *output); +int ToProtoData(kortex_driver::Timeout intput, Timeout *output); +int ToProtoData(kortex_driver::Ssid intput, Ssid *output); +int ToProtoData(kortex_driver::CommunicationInterfaceConfiguration intput, CommunicationInterfaceConfiguration *output); +int ToProtoData(kortex_driver::NetworkHandle intput, NetworkHandle *output); +int ToProtoData(kortex_driver::IPv4Configuration intput, IPv4Configuration *output); +int ToProtoData(kortex_driver::IPv4Information intput, IPv4Information *output); +int ToProtoData(kortex_driver::FullIPv4Configuration intput, FullIPv4Configuration *output); +int ToProtoData(kortex_driver::WifiInformation intput, WifiInformation *output); +int ToProtoData(kortex_driver::WifiInformationList intput, WifiInformationList *output); +int ToProtoData(kortex_driver::WifiConfiguration intput, WifiConfiguration *output); +int ToProtoData(kortex_driver::WifiConfigurationList intput, WifiConfigurationList *output); +int ToProtoData(kortex_driver::ProtectionZoneHandle intput, ProtectionZoneHandle *output); +int ToProtoData(kortex_driver::RotationMatrixRow intput, RotationMatrixRow *output); +int ToProtoData(kortex_driver::RotationMatrix intput, RotationMatrix *output); +int ToProtoData(kortex_driver::Point intput, Point *output); +int ToProtoData(kortex_driver::ZoneShape intput, ZoneShape *output); +int ToProtoData(kortex_driver::ProtectionZone intput, ProtectionZone *output); +int ToProtoData(kortex_driver::ProtectionZoneList intput, ProtectionZoneList *output); +int ToProtoData(kortex_driver::LimitationTypeIdentifier intput, LimitationTypeIdentifier *output); +int ToProtoData(kortex_driver::CartesianLimitation intput, CartesianLimitation *output); +int ToProtoData(kortex_driver::CartesianLimitationList intput, CartesianLimitationList *output); +int ToProtoData(kortex_driver::JointLimitationValue intput, JointLimitationValue *output); +int ToProtoData(kortex_driver::JointLimitationValueList intput, JointLimitationValueList *output); +int ToProtoData(kortex_driver::JointLimitation intput, JointLimitation *output); +int ToProtoData(kortex_driver::JointLimitationTypeIdentifier intput, JointLimitationTypeIdentifier *output); +int ToProtoData(kortex_driver::Query intput, Query *output); +int ToProtoData(kortex_driver::ConfigurationChangeNotification intput, ConfigurationChangeNotification *output); +int ToProtoData(kortex_driver::MappingInfoNotification intput, MappingInfoNotification *output); +int ToProtoData(kortex_driver::ControlModeInformation intput, ControlModeInformation *output); +int ToProtoData(kortex_driver::ControlModeNotification intput, ControlModeNotification *output); +int ToProtoData(kortex_driver::ServoingModeInformation intput, ServoingModeInformation *output); +int ToProtoData(kortex_driver::OperatingModeInformation intput, OperatingModeInformation *output); +int ToProtoData(kortex_driver::OperatingModeNotification intput, OperatingModeNotification *output); +int ToProtoData(kortex_driver::ServoingModeNotification intput, ServoingModeNotification *output); +int ToProtoData(kortex_driver::SequenceInfoNotification intput, SequenceInfoNotification *output); +int ToProtoData(kortex_driver::SequenceInformation intput, SequenceInformation *output); +int ToProtoData(kortex_driver::ProtectionZoneNotification intput, ProtectionZoneNotification *output); +int ToProtoData(kortex_driver::ProtectionZoneInformation intput, ProtectionZoneInformation *output); +int ToProtoData(kortex_driver::UserNotification intput, UserNotification *output); +int ToProtoData(kortex_driver::ControllerHandle intput, ControllerHandle *output); +int ToProtoData(kortex_driver::ControllerElementHandle intput, ControllerElementHandle *output); +int ToProtoData(kortex_driver::ControllerNotification intput, ControllerNotification *output); +int ToProtoData(kortex_driver::ControllerList intput, ControllerList *output); +int ToProtoData(kortex_driver::ControllerState intput, ControllerState *output); +int ToProtoData(kortex_driver::ControllerElementState intput, ControllerElementState *output); +int ToProtoData(kortex_driver::ActionNotification intput, ActionNotification *output); +int ToProtoData(kortex_driver::ActionExecutionState intput, ActionExecutionState *output); +int ToProtoData(kortex_driver::RobotEventNotification intput, RobotEventNotification *output); +int ToProtoData(kortex_driver::FactoryNotification intput, FactoryNotification *output); +int ToProtoData(kortex_driver::NetworkNotification intput, NetworkNotification *output); +int ToProtoData(kortex_driver::ConfigurationChangeNotificationList intput, ConfigurationChangeNotificationList *output); +int ToProtoData(kortex_driver::MappingInfoNotificationList intput, MappingInfoNotificationList *output); +int ToProtoData(kortex_driver::ControlModeNotificationList intput, ControlModeNotificationList *output); +int ToProtoData(kortex_driver::OperatingModeNotificationList intput, OperatingModeNotificationList *output); +int ToProtoData(kortex_driver::ServoingModeNotificationList intput, ServoingModeNotificationList *output); +int ToProtoData(kortex_driver::SequenceInfoNotificationList intput, SequenceInfoNotificationList *output); +int ToProtoData(kortex_driver::ProtectionZoneNotificationList intput, ProtectionZoneNotificationList *output); +int ToProtoData(kortex_driver::UserNotificationList intput, UserNotificationList *output); +int ToProtoData(kortex_driver::SafetyNotificationList intput, SafetyNotificationList *output); +int ToProtoData(kortex_driver::ControllerNotificationList intput, ControllerNotificationList *output); +int ToProtoData(kortex_driver::ActionNotificationList intput, ActionNotificationList *output); +int ToProtoData(kortex_driver::RobotEventNotificationList intput, RobotEventNotificationList *output); +int ToProtoData(kortex_driver::NetworkNotificationList intput, NetworkNotificationList *output); +int ToProtoData(kortex_driver::MappingHandle intput, MappingHandle *output); +int ToProtoData(kortex_driver::SafetyEvent intput, SafetyEvent *output); +int ToProtoData(kortex_driver::ControllerEvent intput, ControllerEvent *output); +int ToProtoData(kortex_driver::GpioEvent intput, GpioEvent *output); +int ToProtoData(kortex_driver::MapEvent intput, MapEvent *output); +int ToProtoData(kortex_driver::MapElement intput, MapElement *output); +int ToProtoData(kortex_driver::ActivateMapHandle intput, ActivateMapHandle *output); +int ToProtoData(kortex_driver::Map intput, Map *output); +int ToProtoData(kortex_driver::MapHandle intput, MapHandle *output); +int ToProtoData(kortex_driver::MapList intput, MapList *output); +int ToProtoData(kortex_driver::MapGroupHandle intput, MapGroupHandle *output); +int ToProtoData(kortex_driver::MapGroup intput, MapGroup *output); +int ToProtoData(kortex_driver::MapGroupList intput, MapGroupList *output); +int ToProtoData(kortex_driver::Mapping intput, Mapping *output); +int ToProtoData(kortex_driver::MappingList intput, MappingList *output); +int ToProtoData(kortex_driver::TransformationMatrix intput, TransformationMatrix *output); +int ToProtoData(kortex_driver::TransformationRow intput, TransformationRow *output); +int ToProtoData(kortex_driver::Pose intput, Pose *output); +int ToProtoData(kortex_driver::Position intput, Position *output); +int ToProtoData(kortex_driver::Orientation intput, Orientation *output); +int ToProtoData(kortex_driver::CartesianSpeed intput, CartesianSpeed *output); +int ToProtoData(kortex_driver::CartesianTrajectoryConstraint intput, CartesianTrajectoryConstraint *output); +int ToProtoData(kortex_driver::JointTrajectoryConstraint intput, JointTrajectoryConstraint *output); +int ToProtoData(kortex_driver::Twist intput, Twist *output); +int ToProtoData(kortex_driver::Admittance intput, Admittance *output); +int ToProtoData(kortex_driver::CartesianReferenceFrameRequest intput, CartesianReferenceFrameRequest *output); +int ToProtoData(kortex_driver::ConstrainedPose intput, ConstrainedPose *output); +int ToProtoData(kortex_driver::ConstrainedPosition intput, ConstrainedPosition *output); +int ToProtoData(kortex_driver::ConstrainedOrientation intput, ConstrainedOrientation *output); +int ToProtoData(kortex_driver::TwistCommand intput, TwistCommand *output); +int ToProtoData(kortex_driver::ConstrainedJointAngles intput, ConstrainedJointAngles *output); +int ToProtoData(kortex_driver::ConstrainedJointAngle intput, ConstrainedJointAngle *output); +int ToProtoData(kortex_driver::JointAngles intput, JointAngles *output); +int ToProtoData(kortex_driver::JointAngle intput, JointAngle *output); +int ToProtoData(kortex_driver::JointSpeeds intput, JointSpeeds *output); +int ToProtoData(kortex_driver::JointSpeed intput, JointSpeed *output); +int ToProtoData(kortex_driver::GripperCommand intput, GripperCommand *output); +int ToProtoData(kortex_driver::GripperRequest intput, GripperRequest *output); +int ToProtoData(kortex_driver::Gripper intput, Gripper *output); +int ToProtoData(kortex_driver::Finger intput, Finger *output); +int ToProtoData(kortex_driver::SystemTime intput, SystemTime *output); +int ToProtoData(kortex_driver::ActuatorInformation intput, ActuatorInformation *output); +int ToProtoData(kortex_driver::ArmStateInformation intput, ArmStateInformation *output); +int ToProtoData(kortex_driver::ArmStateNotification intput, ArmStateNotification *output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/base_ros_converter.cpp b/kortex_driver/src/base_ros_converter.cpp new file mode 100644 index 00000000..c25b3da9 --- /dev/null +++ b/kortex_driver/src/base_ros_converter.cpp @@ -0,0 +1,1378 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "base_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(FullUserProfile input, kortex_driver::FullUserProfile &output) +{ + ToRosData(input.user_profile(), output.user_profile); + output.password = input.password(); + + return 0; +} +int ToRosData(UserProfile input, kortex_driver::UserProfile &output) +{ + ToRosData(input.handle(), output.handle); + output.username = input.username(); + output.firstname = input.firstname(); + output.lastname = input.lastname(); + output.application_data = input.application_data(); + + return 0; +} +int ToRosData(UserProfileList input, kortex_driver::UserProfileList &output) +{ + output.user_profiles.clear(); + for(int i = 0; i < input.user_profiles_size(); i++) + { + kortex_driver::UserProfile temp; + ToRosData(input.user_profiles(i), temp); + output.user_profiles.push_back(temp); + } + + return 0; +} +int ToRosData(UserList input, kortex_driver::UserList &output) +{ + output.user_handles.clear(); + for(int i = 0; i < input.user_handles_size(); i++) + { + kortex_driver::UserProfileHandle temp; + ToRosData(input.user_handles(i), temp); + output.user_handles.push_back(temp); + } + + return 0; +} +int ToRosData(PasswordChange input, kortex_driver::PasswordChange &output) +{ + ToRosData(input.handle(), output.handle); + output.old_password = input.old_password(); + output.new_password = input.new_password(); + + return 0; +} +int ToRosData(SequenceHandle input, kortex_driver::SequenceHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(AdvancedSequenceHandle input, kortex_driver::AdvancedSequenceHandle &output) +{ + ToRosData(input.handle(), output.handle); + output.in_loop = input.in_loop(); + + return 0; +} +int ToRosData(SequenceTaskHandle input, kortex_driver::SequenceTaskHandle &output) +{ + ToRosData(input.sequence_handle(), output.sequence_handle); + output.task_index = input.task_index(); + + return 0; +} +int ToRosData(SequenceTask input, kortex_driver::SequenceTask &output) +{ + output.group_identifier = input.group_identifier(); + ToRosData(input.action(), output.action); + output.application_data = input.application_data(); + + return 0; +} +int ToRosData(Sequence input, kortex_driver::Sequence &output) +{ + ToRosData(input.handle(), output.handle); + output.name = input.name(); + output.application_data = input.application_data(); + output.tasks.clear(); + for(int i = 0; i < input.tasks_size(); i++) + { + kortex_driver::SequenceTask temp; + ToRosData(input.tasks(i), temp); + output.tasks.push_back(temp); + } + + return 0; +} +int ToRosData(SequenceList input, kortex_driver::SequenceList &output) +{ + output.sequence_list.clear(); + for(int i = 0; i < input.sequence_list_size(); i++) + { + kortex_driver::Sequence temp; + ToRosData(input.sequence_list(i), temp); + output.sequence_list.push_back(temp); + } + + return 0; +} +int ToRosData(AppendActionInformation input, kortex_driver::AppendActionInformation &output) +{ + ToRosData(input.sequence_handle(), output.sequence_handle); + ToRosData(input.action(), output.action); + + return 0; +} +int ToRosData(ActionHandle input, kortex_driver::ActionHandle &output) +{ + output.identifier = input.identifier(); + output.action_type = input.action_type(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(RequestedActionType input, kortex_driver::RequestedActionType &output) +{ + output.action_type = input.action_type(); + + return 0; +} +int ToRosData(Action input, kortex_driver::Action &output) +{ + ToRosData(input.handle(), output.handle); + output.name = input.name(); + output.application_data = input.application_data(); + + + auto oneof_type = input.action_parameters_case(); + + switch(oneof_type) + { + case Action::kSendTwistCommand: + { + kortex_driver::TwistCommand temp; + ToRosData(input.send_twist_command(), temp); + output.oneof_action_parameters.send_twist_command.push_back(temp); + break; + } + + case Action::kSendJointSpeeds: + { + kortex_driver::JointSpeeds temp; + ToRosData(input.send_joint_speeds(), temp); + output.oneof_action_parameters.send_joint_speeds.push_back(temp); + break; + } + + case Action::kReachPose: + { + kortex_driver::ConstrainedPose temp; + ToRosData(input.reach_pose(), temp); + output.oneof_action_parameters.reach_pose.push_back(temp); + break; + } + + case Action::kReachJointAngles: + { + kortex_driver::ConstrainedJointAngles temp; + ToRosData(input.reach_joint_angles(), temp); + output.oneof_action_parameters.reach_joint_angles.push_back(temp); + break; + } + + case Action::kToggleAdmittanceMode: + { + output.oneof_action_parameters.toggle_admittance_mode.push_back(input.toggle_admittance_mode()); + + break; + } + + case Action::kSwitchControlMapping: + { + kortex_driver::SwitchControlMapping temp; + ToRosData(input.switch_control_mapping(), temp); + output.oneof_action_parameters.switch_control_mapping.push_back(temp); + break; + } + + case Action::kNavigateJoints: + { + output.oneof_action_parameters.navigate_joints.push_back(input.navigate_joints()); + + break; + } + + case Action::kNavigateMappings: + { + output.oneof_action_parameters.navigate_mappings.push_back(input.navigate_mappings()); + + break; + } + + case Action::kChangeTwist: + { + kortex_driver::ChangeTwist temp; + ToRosData(input.change_twist(), temp); + output.oneof_action_parameters.change_twist.push_back(temp); + break; + } + + case Action::kChangeJointSpeeds: + { + kortex_driver::ChangeJointSpeeds temp; + ToRosData(input.change_joint_speeds(), temp); + output.oneof_action_parameters.change_joint_speeds.push_back(temp); + break; + } + + case Action::kApplyEmergencyStop: + { + kortex_driver::EmergencyStop temp; + ToRosData(input.apply_emergency_stop(), temp); + output.oneof_action_parameters.apply_emergency_stop.push_back(temp); + break; + } + + case Action::kClearFaults: + { + kortex_driver::Faults temp; + ToRosData(input.clear_faults(), temp); + output.oneof_action_parameters.clear_faults.push_back(temp); + break; + } + + case Action::kDelay: + { + kortex_driver::Delay temp; + ToRosData(input.delay(), temp); + output.oneof_action_parameters.delay.push_back(temp); + break; + } + + case Action::kExecuteAction: + { + kortex_driver::ActionHandle temp; + ToRosData(input.execute_action(), temp); + output.oneof_action_parameters.execute_action.push_back(temp); + break; + } + + case Action::kSendGripperCommand: + { + kortex_driver::GripperCommand temp; + ToRosData(input.send_gripper_command(), temp); + output.oneof_action_parameters.send_gripper_command.push_back(temp); + break; + } + + case Action::kStopAction: + { + kortex_driver::Stop temp; + ToRosData(input.stop_action(), temp); + output.oneof_action_parameters.stop_action.push_back(temp); + break; + } + + } + return 0; +} +int ToRosData(SwitchControlMapping input, kortex_driver::SwitchControlMapping &output) +{ + output.controller_identifier = input.controller_identifier(); + ToRosData(input.map_group_handle(), output.map_group_handle); + ToRosData(input.map_handle(), output.map_handle); + + return 0; +} +int ToRosData(ChangeTwist input, kortex_driver::ChangeTwist &output) +{ + output.linear = input.linear(); + output.angular = input.angular(); + + return 0; +} +int ToRosData(ChangeJointSpeeds input, kortex_driver::ChangeJointSpeeds &output) +{ + ToRosData(input.joint_speeds(), output.joint_speeds); + + return 0; +} +int ToRosData(EmergencyStop input, kortex_driver::EmergencyStop &output) +{ + + return 0; +} +int ToRosData(Faults input, kortex_driver::Faults &output) +{ + + return 0; +} +int ToRosData(Delay input, kortex_driver::Delay &output) +{ + output.duration = input.duration(); + + return 0; +} +int ToRosData(Stop input, kortex_driver::Stop &output) +{ + + return 0; +} +int ToRosData(ActionList input, kortex_driver::ActionList &output) +{ + output.action_list.clear(); + for(int i = 0; i < input.action_list_size(); i++) + { + kortex_driver::Action temp; + ToRosData(input.action_list(i), temp); + output.action_list.push_back(temp); + } + + return 0; +} +int ToRosData(Timeout input, kortex_driver::Timeout &output) +{ + output.value = input.value(); + + return 0; +} +int ToRosData(Ssid input, kortex_driver::Ssid &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(CommunicationInterfaceConfiguration input, kortex_driver::CommunicationInterfaceConfiguration &output) +{ + output.type = input.type(); + output.enable = input.enable(); + + return 0; +} +int ToRosData(NetworkHandle input, kortex_driver::NetworkHandle &output) +{ + output.type = input.type(); + + return 0; +} +int ToRosData(IPv4Configuration input, kortex_driver::IPv4Configuration &output) +{ + output.ip_address = input.ip_address(); + output.subnet_mask = input.subnet_mask(); + output.default_gateway = input.default_gateway(); + output.dhcp_enabled = input.dhcp_enabled(); + + return 0; +} +int ToRosData(IPv4Information input, kortex_driver::IPv4Information &output) +{ + output.ip_address = input.ip_address(); + output.subnet_mask = input.subnet_mask(); + output.default_gateway = input.default_gateway(); + + return 0; +} +int ToRosData(FullIPv4Configuration input, kortex_driver::FullIPv4Configuration &output) +{ + ToRosData(input.handle(), output.handle); + ToRosData(input.ipv4_configuration(), output.ipv4_configuration); + + return 0; +} +int ToRosData(WifiInformation input, kortex_driver::WifiInformation &output) +{ + ToRosData(input.ssid(), output.ssid); + output.security_type = input.security_type(); + output.encryption_type = input.encryption_type(); + output.signal_quality = input.signal_quality(); + output.signal_strength = input.signal_strength(); + output.frequency = input.frequency(); + output.channel = input.channel(); + + return 0; +} +int ToRosData(WifiInformationList input, kortex_driver::WifiInformationList &output) +{ + output.wifi_information_list.clear(); + for(int i = 0; i < input.wifi_information_list_size(); i++) + { + kortex_driver::WifiInformation temp; + ToRosData(input.wifi_information_list(i), temp); + output.wifi_information_list.push_back(temp); + } + + return 0; +} +int ToRosData(WifiConfiguration input, kortex_driver::WifiConfiguration &output) +{ + ToRosData(input.ssid(), output.ssid); + output.security_key = input.security_key(); + output.connect_automatically = input.connect_automatically(); + + return 0; +} +int ToRosData(WifiConfigurationList input, kortex_driver::WifiConfigurationList &output) +{ + output.wifi_configuration_list.clear(); + for(int i = 0; i < input.wifi_configuration_list_size(); i++) + { + kortex_driver::WifiConfiguration temp; + ToRosData(input.wifi_configuration_list(i), temp); + output.wifi_configuration_list.push_back(temp); + } + + return 0; +} +int ToRosData(ProtectionZoneHandle input, kortex_driver::ProtectionZoneHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(RotationMatrixRow input, kortex_driver::RotationMatrixRow &output) +{ + output.column1 = input.column1(); + output.column2 = input.column2(); + output.column3 = input.column3(); + + return 0; +} +int ToRosData(RotationMatrix input, kortex_driver::RotationMatrix &output) +{ + ToRosData(input.row1(), output.row1); + ToRosData(input.row2(), output.row2); + ToRosData(input.row3(), output.row3); + + return 0; +} +int ToRosData(Point input, kortex_driver::Point &output) +{ + output.x = input.x(); + output.y = input.y(); + output.z = input.z(); + + return 0; +} +int ToRosData(ZoneShape input, kortex_driver::ZoneShape &output) +{ + output.shape_type = input.shape_type(); + ToRosData(input.origin(), output.origin); + ToRosData(input.orientation(), output.orientation); + + output.dimensions.clear(); + for(int i = 0; i < input.dimensions_size(); i++) + { + output.dimensions.push_back(input.dimensions(i)); + } + output.envelope_thickness = input.envelope_thickness(); + + return 0; +} +int ToRosData(ProtectionZone input, kortex_driver::ProtectionZone &output) +{ + ToRosData(input.handle(), output.handle); + output.name = input.name(); + output.application_data = input.application_data(); + output.is_enabled = input.is_enabled(); + ToRosData(input.shape(), output.shape); + output.limitations.clear(); + for(int i = 0; i < input.limitations_size(); i++) + { + kortex_driver::CartesianLimitation temp; + ToRosData(input.limitations(i), temp); + output.limitations.push_back(temp); + } + output.envelope_limitations.clear(); + for(int i = 0; i < input.envelope_limitations_size(); i++) + { + kortex_driver::CartesianLimitation temp; + ToRosData(input.envelope_limitations(i), temp); + output.envelope_limitations.push_back(temp); + } + + return 0; +} +int ToRosData(ProtectionZoneList input, kortex_driver::ProtectionZoneList &output) +{ + output.protection_zones.clear(); + for(int i = 0; i < input.protection_zones_size(); i++) + { + kortex_driver::ProtectionZone temp; + ToRosData(input.protection_zones(i), temp); + output.protection_zones.push_back(temp); + } + + return 0; +} +int ToRosData(LimitationTypeIdentifier input, kortex_driver::LimitationTypeIdentifier &output) +{ + output.type = input.type(); + + return 0; +} +int ToRosData(CartesianLimitation input, kortex_driver::CartesianLimitation &output) +{ + output.type = input.type(); + output.translation = input.translation(); + output.orientation = input.orientation(); + + return 0; +} +int ToRosData(CartesianLimitationList input, kortex_driver::CartesianLimitationList &output) +{ + output.limitations.clear(); + for(int i = 0; i < input.limitations_size(); i++) + { + kortex_driver::CartesianLimitation temp; + ToRosData(input.limitations(i), temp); + output.limitations.push_back(temp); + } + + return 0; +} +int ToRosData(JointLimitationValue input, kortex_driver::JointLimitationValue &output) +{ + output.type = input.type(); + output.value = input.value(); + + return 0; +} +int ToRosData(JointLimitationValueList input, kortex_driver::JointLimitationValueList &output) +{ + output.joint_limitation_values.clear(); + for(int i = 0; i < input.joint_limitation_values_size(); i++) + { + kortex_driver::JointLimitationValue temp; + ToRosData(input.joint_limitation_values(i), temp); + output.joint_limitation_values.push_back(temp); + } + + return 0; +} +int ToRosData(JointLimitation input, kortex_driver::JointLimitation &output) +{ + output.device_identifier = input.device_identifier(); + ToRosData(input.limitation_value(), output.limitation_value); + + return 0; +} +int ToRosData(JointLimitationTypeIdentifier input, kortex_driver::JointLimitationTypeIdentifier &output) +{ + output.device_identifier = input.device_identifier(); + output.type = input.type(); + + return 0; +} +int ToRosData(Query input, kortex_driver::Query &output) +{ + ToRosData(input.start_timestamp(), output.start_timestamp); + ToRosData(input.end_timestamp(), output.end_timestamp); + output.username = input.username(); + + return 0; +} +int ToRosData(ConfigurationChangeNotification input, kortex_driver::ConfigurationChangeNotification &output) +{ + output.event = input.event(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(MappingInfoNotification input, kortex_driver::MappingInfoNotification &output) +{ + output.controller_identifier = input.controller_identifier(); + ToRosData(input.active_map_handle(), output.active_map_handle); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ControlModeInformation input, kortex_driver::ControlModeInformation &output) +{ + output.mode = input.mode(); + + return 0; +} +int ToRosData(ControlModeNotification input, kortex_driver::ControlModeNotification &output) +{ + output.control_mode = input.control_mode(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ServoingModeInformation input, kortex_driver::ServoingModeInformation &output) +{ + output.servoing_mode = input.servoing_mode(); + + return 0; +} +int ToRosData(OperatingModeInformation input, kortex_driver::OperatingModeInformation &output) +{ + output.operating_mode = input.operating_mode(); + ToRosData(input.device_handle(), output.device_handle); + + return 0; +} +int ToRosData(OperatingModeNotification input, kortex_driver::OperatingModeNotification &output) +{ + output.operating_mode = input.operating_mode(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + ToRosData(input.device_handle(), output.device_handle); + + return 0; +} +int ToRosData(ServoingModeNotification input, kortex_driver::ServoingModeNotification &output) +{ + output.servoing_mode = input.servoing_mode(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(SequenceInfoNotification input, kortex_driver::SequenceInfoNotification &output) +{ + output.event_identifier = input.event_identifier(); + ToRosData(input.sequence_handle(), output.sequence_handle); + output.task_index = input.task_index(); + output.group_identifier = input.group_identifier(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + output.abort_details = input.abort_details(); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(SequenceInformation input, kortex_driver::SequenceInformation &output) +{ + output.event_identifier = input.event_identifier(); + output.task_index = input.task_index(); + output.task_identifier = input.task_identifier(); + + return 0; +} +int ToRosData(ProtectionZoneNotification input, kortex_driver::ProtectionZoneNotification &output) +{ + output.event = input.event(); + ToRosData(input.handle(), output.handle); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ProtectionZoneInformation input, kortex_driver::ProtectionZoneInformation &output) +{ + output.event = input.event(); + + return 0; +} +int ToRosData(UserNotification input, kortex_driver::UserNotification &output) +{ + output.user_event = input.user_event(); + ToRosData(input.modified_user(), output.modified_user); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ControllerHandle input, kortex_driver::ControllerHandle &output) +{ + output.type = input.type(); + output.controller_identifier = input.controller_identifier(); + + return 0; +} +int ToRosData(ControllerElementHandle input, kortex_driver::ControllerElementHandle &output) +{ + ToRosData(input.controller_handle(), output.controller_handle); + + + auto oneof_type = input.identifier_case(); + + switch(oneof_type) + { + case ControllerElementHandle::kButton: + { + break; + } + + case ControllerElementHandle::kAxis: + { + break; + } + + } + return 0; +} +int ToRosData(ControllerNotification input, kortex_driver::ControllerNotification &output) +{ + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ControllerList input, kortex_driver::ControllerList &output) +{ + output.handles.clear(); + for(int i = 0; i < input.handles_size(); i++) + { + kortex_driver::ControllerHandle temp; + ToRosData(input.handles(i), temp); + output.handles.push_back(temp); + } + + return 0; +} +int ToRosData(ControllerState input, kortex_driver::ControllerState &output) +{ + ToRosData(input.handle(), output.handle); + output.event_type = input.event_type(); + + return 0; +} +int ToRosData(ControllerElementState input, kortex_driver::ControllerElementState &output) +{ + ToRosData(input.handle(), output.handle); + output.event_type = input.event_type(); + output.axis_value = input.axis_value(); + + return 0; +} +int ToRosData(ActionNotification input, kortex_driver::ActionNotification &output) +{ + output.action_event = input.action_event(); + ToRosData(input.handle(), output.handle); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + output.abort_details = input.abort_details(); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ActionExecutionState input, kortex_driver::ActionExecutionState &output) +{ + output.action_event = input.action_event(); + ToRosData(input.handle(), output.handle); + + return 0; +} +int ToRosData(RobotEventNotification input, kortex_driver::RobotEventNotification &output) +{ + output.event = input.event(); + ToRosData(input.handle(), output.handle); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(FactoryNotification input, kortex_driver::FactoryNotification &output) +{ + output.event = input.event(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(NetworkNotification input, kortex_driver::NetworkNotification &output) +{ + output.event = input.event(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ConfigurationChangeNotificationList input, kortex_driver::ConfigurationChangeNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ConfigurationChangeNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(MappingInfoNotificationList input, kortex_driver::MappingInfoNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::MappingInfoNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(ControlModeNotificationList input, kortex_driver::ControlModeNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ControlModeNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(OperatingModeNotificationList input, kortex_driver::OperatingModeNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::OperatingModeNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(ServoingModeNotificationList input, kortex_driver::ServoingModeNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ServoingModeNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(SequenceInfoNotificationList input, kortex_driver::SequenceInfoNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::SequenceInfoNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(ProtectionZoneNotificationList input, kortex_driver::ProtectionZoneNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ProtectionZoneNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(UserNotificationList input, kortex_driver::UserNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::UserNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(SafetyNotificationList input, kortex_driver::SafetyNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::SafetyNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(ControllerNotificationList input, kortex_driver::ControllerNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ControllerNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(ActionNotificationList input, kortex_driver::ActionNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::ActionNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(RobotEventNotificationList input, kortex_driver::RobotEventNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::RobotEventNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(NetworkNotificationList input, kortex_driver::NetworkNotificationList &output) +{ + output.notifications.clear(); + for(int i = 0; i < input.notifications_size(); i++) + { + kortex_driver::NetworkNotification temp; + ToRosData(input.notifications(i), temp); + output.notifications.push_back(temp); + } + + return 0; +} +int ToRosData(MappingHandle input, kortex_driver::MappingHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(SafetyEvent input, kortex_driver::SafetyEvent &output) +{ + ToRosData(input.safety_handle(), output.safety_handle); + + return 0; +} +int ToRosData(ControllerEvent input, kortex_driver::ControllerEvent &output) +{ + output.input_type = input.input_type(); + output.behavior = input.behavior(); + output.input_identifier = input.input_identifier(); + + return 0; +} +int ToRosData(GpioEvent input, kortex_driver::GpioEvent &output) +{ + output.gpio_state = input.gpio_state(); + output.device_identifier = input.device_identifier(); + + return 0; +} +int ToRosData(MapEvent input, kortex_driver::MapEvent &output) +{ + output.name = input.name(); + + return 0; +} +int ToRosData(MapElement input, kortex_driver::MapElement &output) +{ + ToRosData(input.event(), output.event); + ToRosData(input.action(), output.action); + + return 0; +} +int ToRosData(ActivateMapHandle input, kortex_driver::ActivateMapHandle &output) +{ + ToRosData(input.mapping_handle(), output.mapping_handle); + ToRosData(input.map_group_handle(), output.map_group_handle); + ToRosData(input.map_handle(), output.map_handle); + + return 0; +} +int ToRosData(Map input, kortex_driver::Map &output) +{ + ToRosData(input.handle(), output.handle); + output.name = input.name(); + output.elements.clear(); + for(int i = 0; i < input.elements_size(); i++) + { + kortex_driver::MapElement temp; + ToRosData(input.elements(i), temp); + output.elements.push_back(temp); + } + + return 0; +} +int ToRosData(MapHandle input, kortex_driver::MapHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(MapList input, kortex_driver::MapList &output) +{ + output.map_list.clear(); + for(int i = 0; i < input.map_list_size(); i++) + { + kortex_driver::Map temp; + ToRosData(input.map_list(i), temp); + output.map_list.push_back(temp); + } + + return 0; +} +int ToRosData(MapGroupHandle input, kortex_driver::MapGroupHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(MapGroup input, kortex_driver::MapGroup &output) +{ + ToRosData(input.group_handle(), output.group_handle); + output.name = input.name(); + ToRosData(input.related_mapping_handle(), output.related_mapping_handle); + ToRosData(input.parent_group_handle(), output.parent_group_handle); + output.children_map_group_handles.clear(); + for(int i = 0; i < input.children_map_group_handles_size(); i++) + { + kortex_driver::MapGroupHandle temp; + ToRosData(input.children_map_group_handles(i), temp); + output.children_map_group_handles.push_back(temp); + } + output.map_handles.clear(); + for(int i = 0; i < input.map_handles_size(); i++) + { + kortex_driver::MapHandle temp; + ToRosData(input.map_handles(i), temp); + output.map_handles.push_back(temp); + } + output.application_data = input.application_data(); + + return 0; +} +int ToRosData(MapGroupList input, kortex_driver::MapGroupList &output) +{ + output.map_groups.clear(); + for(int i = 0; i < input.map_groups_size(); i++) + { + kortex_driver::MapGroup temp; + ToRosData(input.map_groups(i), temp); + output.map_groups.push_back(temp); + } + + return 0; +} +int ToRosData(Mapping input, kortex_driver::Mapping &output) +{ + ToRosData(input.handle(), output.handle); + output.name = input.name(); + output.controller_identifier = input.controller_identifier(); + ToRosData(input.active_map_group_handle(), output.active_map_group_handle); + output.map_group_handles.clear(); + for(int i = 0; i < input.map_group_handles_size(); i++) + { + kortex_driver::MapGroupHandle temp; + ToRosData(input.map_group_handles(i), temp); + output.map_group_handles.push_back(temp); + } + ToRosData(input.active_map_handle(), output.active_map_handle); + output.map_handles.clear(); + for(int i = 0; i < input.map_handles_size(); i++) + { + kortex_driver::MapHandle temp; + ToRosData(input.map_handles(i), temp); + output.map_handles.push_back(temp); + } + output.application_data = input.application_data(); + + return 0; +} +int ToRosData(MappingList input, kortex_driver::MappingList &output) +{ + output.mappings.clear(); + for(int i = 0; i < input.mappings_size(); i++) + { + kortex_driver::Mapping temp; + ToRosData(input.mappings(i), temp); + output.mappings.push_back(temp); + } + + return 0; +} +int ToRosData(TransformationMatrix input, kortex_driver::TransformationMatrix &output) +{ + ToRosData(input.r0(), output.r0); + ToRosData(input.r1(), output.r1); + ToRosData(input.r2(), output.r2); + ToRosData(input.r3(), output.r3); + + return 0; +} +int ToRosData(TransformationRow input, kortex_driver::TransformationRow &output) +{ + output.c0 = input.c0(); + output.c1 = input.c1(); + output.c2 = input.c2(); + output.c3 = input.c3(); + + return 0; +} +int ToRosData(Pose input, kortex_driver::Pose &output) +{ + output.x = input.x(); + output.y = input.y(); + output.z = input.z(); + output.theta_x = input.theta_x(); + output.theta_y = input.theta_y(); + output.theta_z = input.theta_z(); + + return 0; +} +int ToRosData(Position input, kortex_driver::Position &output) +{ + output.x = input.x(); + output.y = input.y(); + output.z = input.z(); + + return 0; +} +int ToRosData(Orientation input, kortex_driver::Orientation &output) +{ + output.theta_x = input.theta_x(); + output.theta_y = input.theta_y(); + output.theta_z = input.theta_z(); + + return 0; +} +int ToRosData(CartesianSpeed input, kortex_driver::CartesianSpeed &output) +{ + output.translation = input.translation(); + output.orientation = input.orientation(); + + return 0; +} +int ToRosData(CartesianTrajectoryConstraint input, kortex_driver::CartesianTrajectoryConstraint &output) +{ + + + auto oneof_type = input.type_case(); + + switch(oneof_type) + { + case CartesianTrajectoryConstraint::kSpeed: + { + kortex_driver::CartesianSpeed temp; + ToRosData(input.speed(), temp); + output.oneof_type.speed.push_back(temp); + break; + } + + case CartesianTrajectoryConstraint::kDuration: + { + break; + } + + } + return 0; +} +int ToRosData(JointTrajectoryConstraint input, kortex_driver::JointTrajectoryConstraint &output) +{ + output.type = input.type(); + output.value = input.value(); + + return 0; +} +int ToRosData(Twist input, kortex_driver::Twist &output) +{ + output.linear_x = input.linear_x(); + output.linear_y = input.linear_y(); + output.linear_z = input.linear_z(); + output.angular_x = input.angular_x(); + output.angular_y = input.angular_y(); + output.angular_z = input.angular_z(); + + return 0; +} +int ToRosData(Admittance input, kortex_driver::Admittance &output) +{ + output.admittance_mode = input.admittance_mode(); + + return 0; +} +int ToRosData(CartesianReferenceFrameRequest input, kortex_driver::CartesianReferenceFrameRequest &output) +{ + output.reference_frame = input.reference_frame(); + + return 0; +} +int ToRosData(ConstrainedPose input, kortex_driver::ConstrainedPose &output) +{ + ToRosData(input.target_pose(), output.target_pose); + ToRosData(input.constraint(), output.constraint); + + return 0; +} +int ToRosData(ConstrainedPosition input, kortex_driver::ConstrainedPosition &output) +{ + ToRosData(input.target_position(), output.target_position); + ToRosData(input.constraint(), output.constraint); + + return 0; +} +int ToRosData(ConstrainedOrientation input, kortex_driver::ConstrainedOrientation &output) +{ + ToRosData(input.target_orientation(), output.target_orientation); + ToRosData(input.constraint(), output.constraint); + + return 0; +} +int ToRosData(TwistCommand input, kortex_driver::TwistCommand &output) +{ + output.mode = input.mode(); + ToRosData(input.twist(), output.twist); + output.duration = input.duration(); + + return 0; +} +int ToRosData(ConstrainedJointAngles input, kortex_driver::ConstrainedJointAngles &output) +{ + ToRosData(input.joint_angles(), output.joint_angles); + ToRosData(input.constraint(), output.constraint); + + return 0; +} +int ToRosData(ConstrainedJointAngle input, kortex_driver::ConstrainedJointAngle &output) +{ + output.joint_identifier = input.joint_identifier(); + output.value = input.value(); + ToRosData(input.constraint(), output.constraint); + + return 0; +} +int ToRosData(JointAngles input, kortex_driver::JointAngles &output) +{ + output.joint_angles.clear(); + for(int i = 0; i < input.joint_angles_size(); i++) + { + kortex_driver::JointAngle temp; + ToRosData(input.joint_angles(i), temp); + output.joint_angles.push_back(temp); + } + + return 0; +} +int ToRosData(JointAngle input, kortex_driver::JointAngle &output) +{ + output.joint_identifier = input.joint_identifier(); + output.value = input.value(); + + return 0; +} +int ToRosData(JointSpeeds input, kortex_driver::JointSpeeds &output) +{ + output.joint_speeds.clear(); + for(int i = 0; i < input.joint_speeds_size(); i++) + { + kortex_driver::JointSpeed temp; + ToRosData(input.joint_speeds(i), temp); + output.joint_speeds.push_back(temp); + } + output.duration = input.duration(); + + return 0; +} +int ToRosData(JointSpeed input, kortex_driver::JointSpeed &output) +{ + output.joint_identifier = input.joint_identifier(); + output.value = input.value(); + output.duration = input.duration(); + + return 0; +} +int ToRosData(GripperCommand input, kortex_driver::GripperCommand &output) +{ + output.mode = input.mode(); + ToRosData(input.gripper(), output.gripper); + output.duration = input.duration(); + + return 0; +} +int ToRosData(GripperRequest input, kortex_driver::GripperRequest &output) +{ + output.mode = input.mode(); + + return 0; +} +int ToRosData(Gripper input, kortex_driver::Gripper &output) +{ + output.finger.clear(); + for(int i = 0; i < input.finger_size(); i++) + { + kortex_driver::Finger temp; + ToRosData(input.finger(i), temp); + output.finger.push_back(temp); + } + + return 0; +} +int ToRosData(Finger input, kortex_driver::Finger &output) +{ + output.finger_identifier = input.finger_identifier(); + output.value = input.value(); + + return 0; +} +int ToRosData(SystemTime input, kortex_driver::SystemTime &output) +{ + output.sec = input.sec(); + output.min = input.min(); + output.hour = input.hour(); + output.mday = input.mday(); + output.mon = input.mon(); + output.year = input.year(); + + return 0; +} +int ToRosData(ActuatorInformation input, kortex_driver::ActuatorInformation &output) +{ + output.count = input.count(); + + return 0; +} +int ToRosData(ArmStateInformation input, kortex_driver::ArmStateInformation &output) +{ + output.active_state = input.active_state(); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(ArmStateNotification input, kortex_driver::ArmStateNotification &output) +{ + output.active_state = input.active_state(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.connection(), output.connection); + + return 0; +} diff --git a/kortex_driver/src/base_ros_converter.h b/kortex_driver/src/base_ros_converter.h new file mode 100644 index 00000000..4e46986b --- /dev/null +++ b/kortex_driver/src/base_ros_converter.h @@ -0,0 +1,308 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BaseROS_CONVERTER_H_ +#define _KORTEX_BaseROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/FullUserProfile.h" +#include "kortex_driver/UserProfile.h" +#include "kortex_driver/UserProfileList.h" +#include "kortex_driver/UserList.h" +#include "kortex_driver/PasswordChange.h" +#include "kortex_driver/SequenceHandle.h" +#include "kortex_driver/AdvancedSequenceHandle.h" +#include "kortex_driver/SequenceTaskHandle.h" +#include "kortex_driver/SequenceTask.h" +#include "kortex_driver/Sequence.h" +#include "kortex_driver/SequenceList.h" +#include "kortex_driver/AppendActionInformation.h" +#include "kortex_driver/ActionHandle.h" +#include "kortex_driver/RequestedActionType.h" +#include "kortex_driver/Action.h" +#include "kortex_driver/SwitchControlMapping.h" +#include "kortex_driver/ChangeTwist.h" +#include "kortex_driver/ChangeJointSpeeds.h" +#include "kortex_driver/EmergencyStop.h" +#include "kortex_driver/Faults.h" +#include "kortex_driver/Delay.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/ActionList.h" +#include "kortex_driver/Timeout.h" +#include "kortex_driver/Ssid.h" +#include "kortex_driver/CommunicationInterfaceConfiguration.h" +#include "kortex_driver/NetworkHandle.h" +#include "kortex_driver/IPv4Configuration.h" +#include "kortex_driver/IPv4Information.h" +#include "kortex_driver/FullIPv4Configuration.h" +#include "kortex_driver/WifiInformation.h" +#include "kortex_driver/WifiInformationList.h" +#include "kortex_driver/WifiConfiguration.h" +#include "kortex_driver/WifiConfigurationList.h" +#include "kortex_driver/ProtectionZoneHandle.h" +#include "kortex_driver/RotationMatrixRow.h" +#include "kortex_driver/RotationMatrix.h" +#include "kortex_driver/Point.h" +#include "kortex_driver/ZoneShape.h" +#include "kortex_driver/ProtectionZone.h" +#include "kortex_driver/ProtectionZoneList.h" +#include "kortex_driver/LimitationTypeIdentifier.h" +#include "kortex_driver/CartesianLimitation.h" +#include "kortex_driver/CartesianLimitationList.h" +#include "kortex_driver/JointLimitationValue.h" +#include "kortex_driver/JointLimitationValueList.h" +#include "kortex_driver/JointLimitation.h" +#include "kortex_driver/JointLimitationTypeIdentifier.h" +#include "kortex_driver/Query.h" +#include "kortex_driver/ConfigurationChangeNotification.h" +#include "kortex_driver/MappingInfoNotification.h" +#include "kortex_driver/ControlModeInformation.h" +#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/ServoingModeInformation.h" +#include "kortex_driver/OperatingModeInformation.h" +#include "kortex_driver/OperatingModeNotification.h" +#include "kortex_driver/ServoingModeNotification.h" +#include "kortex_driver/SequenceInfoNotification.h" +#include "kortex_driver/SequenceInformation.h" +#include "kortex_driver/ProtectionZoneNotification.h" +#include "kortex_driver/ProtectionZoneInformation.h" +#include "kortex_driver/UserNotification.h" +#include "kortex_driver/ControllerHandle.h" +#include "kortex_driver/ControllerElementHandle.h" +#include "kortex_driver/ControllerNotification.h" +#include "kortex_driver/ControllerList.h" +#include "kortex_driver/ControllerState.h" +#include "kortex_driver/ControllerElementState.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionExecutionState.h" +#include "kortex_driver/RobotEventNotification.h" +#include "kortex_driver/FactoryNotification.h" +#include "kortex_driver/NetworkNotification.h" +#include "kortex_driver/ConfigurationChangeNotificationList.h" +#include "kortex_driver/MappingInfoNotificationList.h" +#include "kortex_driver/ControlModeNotificationList.h" +#include "kortex_driver/OperatingModeNotificationList.h" +#include "kortex_driver/ServoingModeNotificationList.h" +#include "kortex_driver/SequenceInfoNotificationList.h" +#include "kortex_driver/ProtectionZoneNotificationList.h" +#include "kortex_driver/UserNotificationList.h" +#include "kortex_driver/SafetyNotificationList.h" +#include "kortex_driver/ControllerNotificationList.h" +#include "kortex_driver/ActionNotificationList.h" +#include "kortex_driver/RobotEventNotificationList.h" +#include "kortex_driver/NetworkNotificationList.h" +#include "kortex_driver/MappingHandle.h" +#include "kortex_driver/SafetyEvent.h" +#include "kortex_driver/ControllerEvent.h" +#include "kortex_driver/GpioEvent.h" +#include "kortex_driver/MapEvent.h" +#include "kortex_driver/MapElement.h" +#include "kortex_driver/ActivateMapHandle.h" +#include "kortex_driver/Map.h" +#include "kortex_driver/MapHandle.h" +#include "kortex_driver/MapList.h" +#include "kortex_driver/MapGroupHandle.h" +#include "kortex_driver/MapGroup.h" +#include "kortex_driver/MapGroupList.h" +#include "kortex_driver/Mapping.h" +#include "kortex_driver/MappingList.h" +#include "kortex_driver/TransformationMatrix.h" +#include "kortex_driver/TransformationRow.h" +#include "kortex_driver/Pose.h" +#include "kortex_driver/Position.h" +#include "kortex_driver/Orientation.h" +#include "kortex_driver/CartesianSpeed.h" +#include "kortex_driver/CartesianTrajectoryConstraint.h" +#include "kortex_driver/JointTrajectoryConstraint.h" +#include "kortex_driver/Twist.h" +#include "kortex_driver/Admittance.h" +#include "kortex_driver/CartesianReferenceFrameRequest.h" +#include "kortex_driver/ConstrainedPose.h" +#include "kortex_driver/ConstrainedPosition.h" +#include "kortex_driver/ConstrainedOrientation.h" +#include "kortex_driver/TwistCommand.h" +#include "kortex_driver/ConstrainedJointAngles.h" +#include "kortex_driver/ConstrainedJointAngle.h" +#include "kortex_driver/JointAngles.h" +#include "kortex_driver/JointAngle.h" +#include "kortex_driver/JointSpeeds.h" +#include "kortex_driver/JointSpeed.h" +#include "kortex_driver/GripperCommand.h" +#include "kortex_driver/GripperRequest.h" +#include "kortex_driver/Gripper.h" +#include "kortex_driver/Finger.h" +#include "kortex_driver/SystemTime.h" +#include "kortex_driver/ActuatorInformation.h" +#include "kortex_driver/ArmStateInformation.h" +#include "kortex_driver/ArmStateNotification.h" + + +using namespace Kinova::Api::Base; + +int ToRosData(FullUserProfile input, kortex_driver::FullUserProfile &output); +int ToRosData(UserProfile input, kortex_driver::UserProfile &output); +int ToRosData(UserProfileList input, kortex_driver::UserProfileList &output); +int ToRosData(UserList input, kortex_driver::UserList &output); +int ToRosData(PasswordChange input, kortex_driver::PasswordChange &output); +int ToRosData(SequenceHandle input, kortex_driver::SequenceHandle &output); +int ToRosData(AdvancedSequenceHandle input, kortex_driver::AdvancedSequenceHandle &output); +int ToRosData(SequenceTaskHandle input, kortex_driver::SequenceTaskHandle &output); +int ToRosData(SequenceTask input, kortex_driver::SequenceTask &output); +int ToRosData(Sequence input, kortex_driver::Sequence &output); +int ToRosData(SequenceList input, kortex_driver::SequenceList &output); +int ToRosData(AppendActionInformation input, kortex_driver::AppendActionInformation &output); +int ToRosData(ActionHandle input, kortex_driver::ActionHandle &output); +int ToRosData(RequestedActionType input, kortex_driver::RequestedActionType &output); +int ToRosData(Action input, kortex_driver::Action &output); +int ToRosData(SwitchControlMapping input, kortex_driver::SwitchControlMapping &output); +int ToRosData(ChangeTwist input, kortex_driver::ChangeTwist &output); +int ToRosData(ChangeJointSpeeds input, kortex_driver::ChangeJointSpeeds &output); +int ToRosData(EmergencyStop input, kortex_driver::EmergencyStop &output); +int ToRosData(Faults input, kortex_driver::Faults &output); +int ToRosData(Delay input, kortex_driver::Delay &output); +int ToRosData(Stop input, kortex_driver::Stop &output); +int ToRosData(ActionList input, kortex_driver::ActionList &output); +int ToRosData(Timeout input, kortex_driver::Timeout &output); +int ToRosData(Ssid input, kortex_driver::Ssid &output); +int ToRosData(CommunicationInterfaceConfiguration input, kortex_driver::CommunicationInterfaceConfiguration &output); +int ToRosData(NetworkHandle input, kortex_driver::NetworkHandle &output); +int ToRosData(IPv4Configuration input, kortex_driver::IPv4Configuration &output); +int ToRosData(IPv4Information input, kortex_driver::IPv4Information &output); +int ToRosData(FullIPv4Configuration input, kortex_driver::FullIPv4Configuration &output); +int ToRosData(WifiInformation input, kortex_driver::WifiInformation &output); +int ToRosData(WifiInformationList input, kortex_driver::WifiInformationList &output); +int ToRosData(WifiConfiguration input, kortex_driver::WifiConfiguration &output); +int ToRosData(WifiConfigurationList input, kortex_driver::WifiConfigurationList &output); +int ToRosData(ProtectionZoneHandle input, kortex_driver::ProtectionZoneHandle &output); +int ToRosData(RotationMatrixRow input, kortex_driver::RotationMatrixRow &output); +int ToRosData(RotationMatrix input, kortex_driver::RotationMatrix &output); +int ToRosData(Point input, kortex_driver::Point &output); +int ToRosData(ZoneShape input, kortex_driver::ZoneShape &output); +int ToRosData(ProtectionZone input, kortex_driver::ProtectionZone &output); +int ToRosData(ProtectionZoneList input, kortex_driver::ProtectionZoneList &output); +int ToRosData(LimitationTypeIdentifier input, kortex_driver::LimitationTypeIdentifier &output); +int ToRosData(CartesianLimitation input, kortex_driver::CartesianLimitation &output); +int ToRosData(CartesianLimitationList input, kortex_driver::CartesianLimitationList &output); +int ToRosData(JointLimitationValue input, kortex_driver::JointLimitationValue &output); +int ToRosData(JointLimitationValueList input, kortex_driver::JointLimitationValueList &output); +int ToRosData(JointLimitation input, kortex_driver::JointLimitation &output); +int ToRosData(JointLimitationTypeIdentifier input, kortex_driver::JointLimitationTypeIdentifier &output); +int ToRosData(Query input, kortex_driver::Query &output); +int ToRosData(ConfigurationChangeNotification input, kortex_driver::ConfigurationChangeNotification &output); +int ToRosData(MappingInfoNotification input, kortex_driver::MappingInfoNotification &output); +int ToRosData(ControlModeInformation input, kortex_driver::ControlModeInformation &output); +int ToRosData(ControlModeNotification input, kortex_driver::ControlModeNotification &output); +int ToRosData(ServoingModeInformation input, kortex_driver::ServoingModeInformation &output); +int ToRosData(OperatingModeInformation input, kortex_driver::OperatingModeInformation &output); +int ToRosData(OperatingModeNotification input, kortex_driver::OperatingModeNotification &output); +int ToRosData(ServoingModeNotification input, kortex_driver::ServoingModeNotification &output); +int ToRosData(SequenceInfoNotification input, kortex_driver::SequenceInfoNotification &output); +int ToRosData(SequenceInformation input, kortex_driver::SequenceInformation &output); +int ToRosData(ProtectionZoneNotification input, kortex_driver::ProtectionZoneNotification &output); +int ToRosData(ProtectionZoneInformation input, kortex_driver::ProtectionZoneInformation &output); +int ToRosData(UserNotification input, kortex_driver::UserNotification &output); +int ToRosData(ControllerHandle input, kortex_driver::ControllerHandle &output); +int ToRosData(ControllerElementHandle input, kortex_driver::ControllerElementHandle &output); +int ToRosData(ControllerNotification input, kortex_driver::ControllerNotification &output); +int ToRosData(ControllerList input, kortex_driver::ControllerList &output); +int ToRosData(ControllerState input, kortex_driver::ControllerState &output); +int ToRosData(ControllerElementState input, kortex_driver::ControllerElementState &output); +int ToRosData(ActionNotification input, kortex_driver::ActionNotification &output); +int ToRosData(ActionExecutionState input, kortex_driver::ActionExecutionState &output); +int ToRosData(RobotEventNotification input, kortex_driver::RobotEventNotification &output); +int ToRosData(FactoryNotification input, kortex_driver::FactoryNotification &output); +int ToRosData(NetworkNotification input, kortex_driver::NetworkNotification &output); +int ToRosData(ConfigurationChangeNotificationList input, kortex_driver::ConfigurationChangeNotificationList &output); +int ToRosData(MappingInfoNotificationList input, kortex_driver::MappingInfoNotificationList &output); +int ToRosData(ControlModeNotificationList input, kortex_driver::ControlModeNotificationList &output); +int ToRosData(OperatingModeNotificationList input, kortex_driver::OperatingModeNotificationList &output); +int ToRosData(ServoingModeNotificationList input, kortex_driver::ServoingModeNotificationList &output); +int ToRosData(SequenceInfoNotificationList input, kortex_driver::SequenceInfoNotificationList &output); +int ToRosData(ProtectionZoneNotificationList input, kortex_driver::ProtectionZoneNotificationList &output); +int ToRosData(UserNotificationList input, kortex_driver::UserNotificationList &output); +int ToRosData(SafetyNotificationList input, kortex_driver::SafetyNotificationList &output); +int ToRosData(ControllerNotificationList input, kortex_driver::ControllerNotificationList &output); +int ToRosData(ActionNotificationList input, kortex_driver::ActionNotificationList &output); +int ToRosData(RobotEventNotificationList input, kortex_driver::RobotEventNotificationList &output); +int ToRosData(NetworkNotificationList input, kortex_driver::NetworkNotificationList &output); +int ToRosData(MappingHandle input, kortex_driver::MappingHandle &output); +int ToRosData(SafetyEvent input, kortex_driver::SafetyEvent &output); +int ToRosData(ControllerEvent input, kortex_driver::ControllerEvent &output); +int ToRosData(GpioEvent input, kortex_driver::GpioEvent &output); +int ToRosData(MapEvent input, kortex_driver::MapEvent &output); +int ToRosData(MapElement input, kortex_driver::MapElement &output); +int ToRosData(ActivateMapHandle input, kortex_driver::ActivateMapHandle &output); +int ToRosData(Map input, kortex_driver::Map &output); +int ToRosData(MapHandle input, kortex_driver::MapHandle &output); +int ToRosData(MapList input, kortex_driver::MapList &output); +int ToRosData(MapGroupHandle input, kortex_driver::MapGroupHandle &output); +int ToRosData(MapGroup input, kortex_driver::MapGroup &output); +int ToRosData(MapGroupList input, kortex_driver::MapGroupList &output); +int ToRosData(Mapping input, kortex_driver::Mapping &output); +int ToRosData(MappingList input, kortex_driver::MappingList &output); +int ToRosData(TransformationMatrix input, kortex_driver::TransformationMatrix &output); +int ToRosData(TransformationRow input, kortex_driver::TransformationRow &output); +int ToRosData(Pose input, kortex_driver::Pose &output); +int ToRosData(Position input, kortex_driver::Position &output); +int ToRosData(Orientation input, kortex_driver::Orientation &output); +int ToRosData(CartesianSpeed input, kortex_driver::CartesianSpeed &output); +int ToRosData(CartesianTrajectoryConstraint input, kortex_driver::CartesianTrajectoryConstraint &output); +int ToRosData(JointTrajectoryConstraint input, kortex_driver::JointTrajectoryConstraint &output); +int ToRosData(Twist input, kortex_driver::Twist &output); +int ToRosData(Admittance input, kortex_driver::Admittance &output); +int ToRosData(CartesianReferenceFrameRequest input, kortex_driver::CartesianReferenceFrameRequest &output); +int ToRosData(ConstrainedPose input, kortex_driver::ConstrainedPose &output); +int ToRosData(ConstrainedPosition input, kortex_driver::ConstrainedPosition &output); +int ToRosData(ConstrainedOrientation input, kortex_driver::ConstrainedOrientation &output); +int ToRosData(TwistCommand input, kortex_driver::TwistCommand &output); +int ToRosData(ConstrainedJointAngles input, kortex_driver::ConstrainedJointAngles &output); +int ToRosData(ConstrainedJointAngle input, kortex_driver::ConstrainedJointAngle &output); +int ToRosData(JointAngles input, kortex_driver::JointAngles &output); +int ToRosData(JointAngle input, kortex_driver::JointAngle &output); +int ToRosData(JointSpeeds input, kortex_driver::JointSpeeds &output); +int ToRosData(JointSpeed input, kortex_driver::JointSpeed &output); +int ToRosData(GripperCommand input, kortex_driver::GripperCommand &output); +int ToRosData(GripperRequest input, kortex_driver::GripperRequest &output); +int ToRosData(Gripper input, kortex_driver::Gripper &output); +int ToRosData(Finger input, kortex_driver::Finger &output); +int ToRosData(SystemTime input, kortex_driver::SystemTime &output); +int ToRosData(ActuatorInformation input, kortex_driver::ActuatorInformation &output); +int ToRosData(ArmStateInformation input, kortex_driver::ArmStateInformation &output); +int ToRosData(ArmStateNotification input, kortex_driver::ArmStateNotification &output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/basecyclic_proto_converter.cpp b/kortex_driver/src/basecyclic_proto_converter.cpp new file mode 100644 index 00000000..b4e14bb8 --- /dev/null +++ b/kortex_driver/src/basecyclic_proto_converter.cpp @@ -0,0 +1,212 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "basecyclic_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_driver::ActuatorCommand input, ActuatorCommand *output) +{ + output->set_command_id(input.command_id); + output->set_flags(input.flags); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_torque_joint(input.torque_joint); + output->set_current_motor(input.current_motor); + + return 0; +} +int ToProtoData(kortex_driver::InterconnectCommand input, InterconnectCommand *output) +{ + output->set_command_id(input.command_id); + output->set_flags(input.flags); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_force(input.force); + + return 0; +} +int ToProtoData(kortex_driver::ActuatorFeedback input, ActuatorFeedback *output) +{ + output->set_command_id(input.command_id); + output->set_status_flags(input.status_flags); + output->set_jitter_comm(input.jitter_comm); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_torque(input.torque); + output->set_current_motor(input.current_motor); + output->set_voltage(input.voltage); + output->set_temperature_motor(input.temperature_motor); + output->set_temperature_core(input.temperature_core); + output->set_fault_bank_a(input.fault_bank_a); + output->set_fault_bank_b(input.fault_bank_b); + output->set_warning_bank_a(input.warning_bank_a); + output->set_warning_bank_b(input.warning_bank_b); + + return 0; +} +int ToProtoData(kortex_driver::InterconnectFeedback input, InterconnectFeedback *output) +{ + output->set_command_id(input.command_id); + output->set_status_flags(input.status_flags); + output->set_jitter_comm(input.jitter_comm); + output->set_position(input.position); + output->set_velocity(input.velocity); + output->set_force(input.force); + output->set_imu_acceleration_x(input.imu_acceleration_x); + output->set_imu_acceleration_y(input.imu_acceleration_y); + output->set_imu_acceleration_z(input.imu_acceleration_z); + output->set_imu_angular_velocity_x(input.imu_angular_velocity_x); + output->set_imu_angular_velocity_y(input.imu_angular_velocity_y); + output->set_imu_angular_velocity_z(input.imu_angular_velocity_z); + output->set_voltage(input.voltage); + output->set_temperature_core(input.temperature_core); + output->set_fault_bank_a(input.fault_bank_a); + output->set_fault_bank_b(input.fault_bank_b); + output->set_warning_bank_a(input.warning_bank_a); + output->set_warning_bank_b(input.warning_bank_b); + + return 0; +} +int ToProtoData(kortex_driver::ActuatorCustomData input, ActuatorCustomData *output) +{ + output->set_command_id(input.command_id); + output->set_custom_data_0(input.custom_data_0); + output->set_custom_data_1(input.custom_data_1); + output->set_custom_data_2(input.custom_data_2); + output->set_custom_data_3(input.custom_data_3); + output->set_custom_data_4(input.custom_data_4); + output->set_custom_data_5(input.custom_data_5); + output->set_custom_data_6(input.custom_data_6); + output->set_custom_data_7(input.custom_data_7); + output->set_custom_data_8(input.custom_data_8); + output->set_custom_data_9(input.custom_data_9); + output->set_custom_data_10(input.custom_data_10); + output->set_custom_data_11(input.custom_data_11); + output->set_custom_data_12(input.custom_data_12); + output->set_custom_data_13(input.custom_data_13); + output->set_custom_data_14(input.custom_data_14); + output->set_custom_data_15(input.custom_data_15); + + return 0; +} +int ToProtoData(kortex_driver::InterconnectCustomData input, InterconnectCustomData *output) +{ + output->set_command_id(input.command_id); + output->set_custom_data_0(input.custom_data_0); + output->set_custom_data_1(input.custom_data_1); + output->set_custom_data_2(input.custom_data_2); + output->set_custom_data_3(input.custom_data_3); + output->set_custom_data_4(input.custom_data_4); + output->set_custom_data_5(input.custom_data_5); + output->set_custom_data_6(input.custom_data_6); + output->set_custom_data_7(input.custom_data_7); + output->set_custom_data_8(input.custom_data_8); + output->set_custom_data_9(input.custom_data_9); + output->set_custom_data_10(input.custom_data_10); + output->set_custom_data_11(input.custom_data_11); + output->set_custom_data_12(input.custom_data_12); + output->set_custom_data_13(input.custom_data_13); + output->set_custom_data_14(input.custom_data_14); + output->set_custom_data_15(input.custom_data_15); + + return 0; +} +int ToProtoData(kortex_driver::BaseFeedback input, BaseFeedback *output) +{ + output->set_active_state_connection_identifier(input.active_state_connection_identifier); + output->set_active_state((Kinova::Api::Common::ArmState)input.active_state); + output->set_arm_voltage(input.arm_voltage); + output->set_arm_current(input.arm_current); + output->set_temperature_cpu(input.temperature_cpu); + output->set_temperature_ambient(input.temperature_ambient); + output->set_imu_acceleration_x(input.imu_acceleration_x); + output->set_imu_acceleration_y(input.imu_acceleration_y); + output->set_imu_acceleration_z(input.imu_acceleration_z); + output->set_imu_angular_velocity_x(input.imu_angular_velocity_x); + output->set_imu_angular_velocity_y(input.imu_angular_velocity_y); + output->set_imu_angular_velocity_z(input.imu_angular_velocity_z); + output->set_tool_pose_x(input.tool_pose_x); + output->set_tool_pose_y(input.tool_pose_y); + output->set_tool_pose_z(input.tool_pose_z); + output->set_tool_pose_theta_x(input.tool_pose_theta_x); + output->set_tool_pose_theta_y(input.tool_pose_theta_y); + output->set_tool_pose_theta_z(input.tool_pose_theta_z); + output->set_tool_twist_linear_x(input.tool_twist_linear_x); + output->set_tool_twist_linear_y(input.tool_twist_linear_y); + output->set_tool_twist_linear_z(input.tool_twist_linear_z); + output->set_tool_twist_angular_x(input.tool_twist_angular_x); + output->set_tool_twist_angular_y(input.tool_twist_angular_y); + output->set_tool_twist_angular_z(input.tool_twist_angular_z); + output->set_tool_external_wrench_force_x(input.tool_external_wrench_force_x); + output->set_tool_external_wrench_force_y(input.tool_external_wrench_force_y); + output->set_tool_external_wrench_force_z(input.tool_external_wrench_force_z); + output->set_tool_external_wrench_torque_x(input.tool_external_wrench_torque_x); + output->set_tool_external_wrench_torque_y(input.tool_external_wrench_torque_y); + output->set_tool_external_wrench_torque_z(input.tool_external_wrench_torque_z); + output->set_fault_bank_a(input.fault_bank_a); + output->set_fault_bank_b(input.fault_bank_b); + output->set_warning_bank_a(input.warning_bank_a); + output->set_warning_bank_b(input.warning_bank_b); + + return 0; +} +int ToProtoData(kortex_driver::CustomData input, CustomData *output) +{ + output->set_frame_id(input.frame_id); + output->set_custom_data_0(input.custom_data_0); + output->set_custom_data_1(input.custom_data_1); + output->set_custom_data_2(input.custom_data_2); + output->set_custom_data_3(input.custom_data_3); + output->set_custom_data_4(input.custom_data_4); + output->set_custom_data_5(input.custom_data_5); + output->set_custom_data_6(input.custom_data_6); + output->set_custom_data_7(input.custom_data_7); + output->clear_actuators_custom_data(); + for(int i = 0; i < input.actuators_custom_data.size(); i++) + { + ToProtoData(input.actuators_custom_data[i], output->add_actuators_custom_data()); + } + ToProtoData(input.interconnect_custom_data, output->mutable_interconnect_custom_data()); + + return 0; +} +int ToProtoData(kortex_driver::Command input, Command *output) +{ + output->set_frame_id(input.frame_id); + output->clear_actuators(); + for(int i = 0; i < input.actuators.size(); i++) + { + ToProtoData(input.actuators[i], output->add_actuators()); + } + ToProtoData(input.interconnect, output->mutable_interconnect()); + + return 0; +} +int ToProtoData(kortex_driver::Feedback input, Feedback *output) +{ + output->set_frame_id(input.frame_id); + ToProtoData(input.base, output->mutable_base()); + output->clear_actuators(); + for(int i = 0; i < input.actuators.size(); i++) + { + ToProtoData(input.actuators[i], output->add_actuators()); + } + ToProtoData(input.interconnect, output->mutable_interconnect()); + + return 0; +} diff --git a/kortex_driver/src/basecyclic_proto_converter.h b/kortex_driver/src/basecyclic_proto_converter.h new file mode 100644 index 00000000..1dc3d079 --- /dev/null +++ b/kortex_driver/src/basecyclic_proto_converter.h @@ -0,0 +1,68 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BaseCyclicPROTO_CONVERTER_H_ +#define _KORTEX_BaseCyclicPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/ActuatorCommand.h" +#include "kortex_driver/InterconnectCommand.h" +#include "kortex_driver/ActuatorFeedback.h" +#include "kortex_driver/InterconnectFeedback.h" +#include "kortex_driver/ActuatorCustomData.h" +#include "kortex_driver/InterconnectCustomData.h" +#include "kortex_driver/BaseFeedback.h" +#include "kortex_driver/CustomData.h" +#include "kortex_driver/Command.h" +#include "kortex_driver/Feedback.h" + + +using namespace Kinova::Api::BaseCyclic; + +int ToProtoData(kortex_driver::ActuatorCommand intput, ActuatorCommand *output); +int ToProtoData(kortex_driver::InterconnectCommand intput, InterconnectCommand *output); +int ToProtoData(kortex_driver::ActuatorFeedback intput, ActuatorFeedback *output); +int ToProtoData(kortex_driver::InterconnectFeedback intput, InterconnectFeedback *output); +int ToProtoData(kortex_driver::ActuatorCustomData intput, ActuatorCustomData *output); +int ToProtoData(kortex_driver::InterconnectCustomData intput, InterconnectCustomData *output); +int ToProtoData(kortex_driver::BaseFeedback intput, BaseFeedback *output); +int ToProtoData(kortex_driver::CustomData intput, CustomData *output); +int ToProtoData(kortex_driver::Command intput, Command *output); +int ToProtoData(kortex_driver::Feedback intput, Feedback *output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/basecyclic_ros_converter.cpp b/kortex_driver/src/basecyclic_ros_converter.cpp new file mode 100644 index 00000000..5283ee52 --- /dev/null +++ b/kortex_driver/src/basecyclic_ros_converter.cpp @@ -0,0 +1,218 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "basecyclic_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(ActuatorCommand input, kortex_driver::ActuatorCommand &output) +{ + output.command_id = input.command_id(); + output.flags = input.flags(); + output.position = input.position(); + output.velocity = input.velocity(); + output.torque_joint = input.torque_joint(); + output.current_motor = input.current_motor(); + + return 0; +} +int ToRosData(InterconnectCommand input, kortex_driver::InterconnectCommand &output) +{ + output.command_id = input.command_id(); + output.flags = input.flags(); + output.position = input.position(); + output.velocity = input.velocity(); + output.force = input.force(); + + return 0; +} +int ToRosData(ActuatorFeedback input, kortex_driver::ActuatorFeedback &output) +{ + output.command_id = input.command_id(); + output.status_flags = input.status_flags(); + output.jitter_comm = input.jitter_comm(); + output.position = input.position(); + output.velocity = input.velocity(); + output.torque = input.torque(); + output.current_motor = input.current_motor(); + output.voltage = input.voltage(); + output.temperature_motor = input.temperature_motor(); + output.temperature_core = input.temperature_core(); + output.fault_bank_a = input.fault_bank_a(); + output.fault_bank_b = input.fault_bank_b(); + output.warning_bank_a = input.warning_bank_a(); + output.warning_bank_b = input.warning_bank_b(); + + return 0; +} +int ToRosData(InterconnectFeedback input, kortex_driver::InterconnectFeedback &output) +{ + output.command_id = input.command_id(); + output.status_flags = input.status_flags(); + output.jitter_comm = input.jitter_comm(); + output.position = input.position(); + output.velocity = input.velocity(); + output.force = input.force(); + output.imu_acceleration_x = input.imu_acceleration_x(); + output.imu_acceleration_y = input.imu_acceleration_y(); + output.imu_acceleration_z = input.imu_acceleration_z(); + output.imu_angular_velocity_x = input.imu_angular_velocity_x(); + output.imu_angular_velocity_y = input.imu_angular_velocity_y(); + output.imu_angular_velocity_z = input.imu_angular_velocity_z(); + output.voltage = input.voltage(); + output.temperature_core = input.temperature_core(); + output.fault_bank_a = input.fault_bank_a(); + output.fault_bank_b = input.fault_bank_b(); + output.warning_bank_a = input.warning_bank_a(); + output.warning_bank_b = input.warning_bank_b(); + + return 0; +} +int ToRosData(ActuatorCustomData input, kortex_driver::ActuatorCustomData &output) +{ + output.command_id = input.command_id(); + output.custom_data_0 = input.custom_data_0(); + output.custom_data_1 = input.custom_data_1(); + output.custom_data_2 = input.custom_data_2(); + output.custom_data_3 = input.custom_data_3(); + output.custom_data_4 = input.custom_data_4(); + output.custom_data_5 = input.custom_data_5(); + output.custom_data_6 = input.custom_data_6(); + output.custom_data_7 = input.custom_data_7(); + output.custom_data_8 = input.custom_data_8(); + output.custom_data_9 = input.custom_data_9(); + output.custom_data_10 = input.custom_data_10(); + output.custom_data_11 = input.custom_data_11(); + output.custom_data_12 = input.custom_data_12(); + output.custom_data_13 = input.custom_data_13(); + output.custom_data_14 = input.custom_data_14(); + output.custom_data_15 = input.custom_data_15(); + + return 0; +} +int ToRosData(InterconnectCustomData input, kortex_driver::InterconnectCustomData &output) +{ + output.command_id = input.command_id(); + output.custom_data_0 = input.custom_data_0(); + output.custom_data_1 = input.custom_data_1(); + output.custom_data_2 = input.custom_data_2(); + output.custom_data_3 = input.custom_data_3(); + output.custom_data_4 = input.custom_data_4(); + output.custom_data_5 = input.custom_data_5(); + output.custom_data_6 = input.custom_data_6(); + output.custom_data_7 = input.custom_data_7(); + output.custom_data_8 = input.custom_data_8(); + output.custom_data_9 = input.custom_data_9(); + output.custom_data_10 = input.custom_data_10(); + output.custom_data_11 = input.custom_data_11(); + output.custom_data_12 = input.custom_data_12(); + output.custom_data_13 = input.custom_data_13(); + output.custom_data_14 = input.custom_data_14(); + output.custom_data_15 = input.custom_data_15(); + + return 0; +} +int ToRosData(BaseFeedback input, kortex_driver::BaseFeedback &output) +{ + output.active_state_connection_identifier = input.active_state_connection_identifier(); + output.active_state = input.active_state(); + output.arm_voltage = input.arm_voltage(); + output.arm_current = input.arm_current(); + output.temperature_cpu = input.temperature_cpu(); + output.temperature_ambient = input.temperature_ambient(); + output.imu_acceleration_x = input.imu_acceleration_x(); + output.imu_acceleration_y = input.imu_acceleration_y(); + output.imu_acceleration_z = input.imu_acceleration_z(); + output.imu_angular_velocity_x = input.imu_angular_velocity_x(); + output.imu_angular_velocity_y = input.imu_angular_velocity_y(); + output.imu_angular_velocity_z = input.imu_angular_velocity_z(); + output.tool_pose_x = input.tool_pose_x(); + output.tool_pose_y = input.tool_pose_y(); + output.tool_pose_z = input.tool_pose_z(); + output.tool_pose_theta_x = input.tool_pose_theta_x(); + output.tool_pose_theta_y = input.tool_pose_theta_y(); + output.tool_pose_theta_z = input.tool_pose_theta_z(); + output.tool_twist_linear_x = input.tool_twist_linear_x(); + output.tool_twist_linear_y = input.tool_twist_linear_y(); + output.tool_twist_linear_z = input.tool_twist_linear_z(); + output.tool_twist_angular_x = input.tool_twist_angular_x(); + output.tool_twist_angular_y = input.tool_twist_angular_y(); + output.tool_twist_angular_z = input.tool_twist_angular_z(); + output.tool_external_wrench_force_x = input.tool_external_wrench_force_x(); + output.tool_external_wrench_force_y = input.tool_external_wrench_force_y(); + output.tool_external_wrench_force_z = input.tool_external_wrench_force_z(); + output.tool_external_wrench_torque_x = input.tool_external_wrench_torque_x(); + output.tool_external_wrench_torque_y = input.tool_external_wrench_torque_y(); + output.tool_external_wrench_torque_z = input.tool_external_wrench_torque_z(); + output.fault_bank_a = input.fault_bank_a(); + output.fault_bank_b = input.fault_bank_b(); + output.warning_bank_a = input.warning_bank_a(); + output.warning_bank_b = input.warning_bank_b(); + + return 0; +} +int ToRosData(CustomData input, kortex_driver::CustomData &output) +{ + output.frame_id = input.frame_id(); + output.custom_data_0 = input.custom_data_0(); + output.custom_data_1 = input.custom_data_1(); + output.custom_data_2 = input.custom_data_2(); + output.custom_data_3 = input.custom_data_3(); + output.custom_data_4 = input.custom_data_4(); + output.custom_data_5 = input.custom_data_5(); + output.custom_data_6 = input.custom_data_6(); + output.custom_data_7 = input.custom_data_7(); + output.actuators_custom_data.clear(); + for(int i = 0; i < input.actuators_custom_data_size(); i++) + { + kortex_driver::ActuatorCustomData temp; + ToRosData(input.actuators_custom_data(i), temp); + output.actuators_custom_data.push_back(temp); + } + ToRosData(input.interconnect_custom_data(), output.interconnect_custom_data); + + return 0; +} +int ToRosData(Command input, kortex_driver::Command &output) +{ + output.frame_id = input.frame_id(); + output.actuators.clear(); + for(int i = 0; i < input.actuators_size(); i++) + { + kortex_driver::ActuatorCommand temp; + ToRosData(input.actuators(i), temp); + output.actuators.push_back(temp); + } + ToRosData(input.interconnect(), output.interconnect); + + return 0; +} +int ToRosData(Feedback input, kortex_driver::Feedback &output) +{ + output.frame_id = input.frame_id(); + ToRosData(input.base(), output.base); + output.actuators.clear(); + for(int i = 0; i < input.actuators_size(); i++) + { + kortex_driver::ActuatorFeedback temp; + ToRosData(input.actuators(i), temp); + output.actuators.push_back(temp); + } + ToRosData(input.interconnect(), output.interconnect); + + return 0; +} diff --git a/kortex_driver/src/basecyclic_ros_converter.h b/kortex_driver/src/basecyclic_ros_converter.h new file mode 100644 index 00000000..b4ab4bc7 --- /dev/null +++ b/kortex_driver/src/basecyclic_ros_converter.h @@ -0,0 +1,68 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_BaseCyclicROS_CONVERTER_H_ +#define _KORTEX_BaseCyclicROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/ActuatorCommand.h" +#include "kortex_driver/InterconnectCommand.h" +#include "kortex_driver/ActuatorFeedback.h" +#include "kortex_driver/InterconnectFeedback.h" +#include "kortex_driver/ActuatorCustomData.h" +#include "kortex_driver/InterconnectCustomData.h" +#include "kortex_driver/BaseFeedback.h" +#include "kortex_driver/CustomData.h" +#include "kortex_driver/Command.h" +#include "kortex_driver/Feedback.h" + + +using namespace Kinova::Api::BaseCyclic; + +int ToRosData(ActuatorCommand input, kortex_driver::ActuatorCommand &output); +int ToRosData(InterconnectCommand input, kortex_driver::InterconnectCommand &output); +int ToRosData(ActuatorFeedback input, kortex_driver::ActuatorFeedback &output); +int ToRosData(InterconnectFeedback input, kortex_driver::InterconnectFeedback &output); +int ToRosData(ActuatorCustomData input, kortex_driver::ActuatorCustomData &output); +int ToRosData(InterconnectCustomData input, kortex_driver::InterconnectCustomData &output); +int ToRosData(BaseFeedback input, kortex_driver::BaseFeedback &output); +int ToRosData(CustomData input, kortex_driver::CustomData &output); +int ToRosData(Command input, kortex_driver::Command &output); +int ToRosData(Feedback input, kortex_driver::Feedback &output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/common_proto_converter.cpp b/kortex_driver/src/common_proto_converter.cpp new file mode 100644 index 00000000..6741b645 --- /dev/null +++ b/kortex_driver/src/common_proto_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_driver::DeviceHandle input, DeviceHandle *output) +{ + output->set_device_type((Kinova::Api::Common::DeviceTypes)input.device_type); + output->set_device_identifier(input.device_identifier); + output->set_order(input.order); + + return 0; +} +int ToProtoData(kortex_driver::Empty input, Empty *output) +{ + + return 0; +} +int ToProtoData(kortex_driver::NotificationOptions input, NotificationOptions *output) +{ + output->set_type((Kinova::Api::Common::NotificationType)input.type); + output->set_rate_m_sec(input.rate_m_sec); + output->set_threshold_value(input.threshold_value); + + return 0; +} +int ToProtoData(kortex_driver::SafetyHandle input, SafetyHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_driver::NotificationHandle input, NotificationHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_driver::SafetyNotification input, SafetyNotification *output) +{ + ToProtoData(input.safety_handle, output->mutable_safety_handle()); + output->set_value((Kinova::Api::Common::SafetyStatusValue)input.value); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_driver::Timestamp input, Timestamp *output) +{ + output->set_sec(input.sec); + output->set_usec(input.usec); + + return 0; +} +int ToProtoData(kortex_driver::UserProfileHandle input, UserProfileHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_driver::Connection input, Connection *output) +{ + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_connection_information(input.connection_information); + output->set_connection_identifier(input.connection_identifier); + + return 0; +} diff --git a/kortex_driver/src/common_proto_converter.h b/kortex_driver/src/common_proto_converter.h new file mode 100644 index 00000000..469401e7 --- /dev/null +++ b/kortex_driver/src/common_proto_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonPROTO_CONVERTER_H_ +#define _KORTEX_CommonPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/DeviceHandle.h" +#include "kortex_driver/Empty.h" +#include "kortex_driver/NotificationOptions.h" +#include "kortex_driver/SafetyHandle.h" +#include "kortex_driver/NotificationHandle.h" +#include "kortex_driver/SafetyNotification.h" +#include "kortex_driver/Timestamp.h" +#include "kortex_driver/UserProfileHandle.h" +#include "kortex_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToProtoData(kortex_driver::DeviceHandle intput, DeviceHandle *output); +int ToProtoData(kortex_driver::Empty intput, Empty *output); +int ToProtoData(kortex_driver::NotificationOptions intput, NotificationOptions *output); +int ToProtoData(kortex_driver::SafetyHandle intput, SafetyHandle *output); +int ToProtoData(kortex_driver::NotificationHandle intput, NotificationHandle *output); +int ToProtoData(kortex_driver::SafetyNotification intput, SafetyNotification *output); +int ToProtoData(kortex_driver::Timestamp intput, Timestamp *output); +int ToProtoData(kortex_driver::UserProfileHandle intput, UserProfileHandle *output); +int ToProtoData(kortex_driver::Connection intput, Connection *output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/common_ros_converter.cpp b/kortex_driver/src/common_ros_converter.cpp new file mode 100644 index 00000000..6cc1bcaf --- /dev/null +++ b/kortex_driver/src/common_ros_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_ros_converter.h" + + +int ToRosData(DeviceHandle input, kortex_driver::DeviceHandle &output) +{ + output.device_type = input.device_type(); + output.device_identifier = input.device_identifier(); + output.order = input.order(); + + return 0; +} +int ToRosData(Empty input, kortex_driver::Empty &output) +{ + + return 0; +} +int ToRosData(NotificationOptions input, kortex_driver::NotificationOptions &output) +{ + output.type = input.type(); + output.rate_m_sec = input.rate_m_sec(); + output.threshold_value = input.threshold_value(); + + return 0; +} +int ToRosData(SafetyHandle input, kortex_driver::SafetyHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(NotificationHandle input, kortex_driver::NotificationHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(SafetyNotification input, kortex_driver::SafetyNotification &output) +{ + ToRosData(input.safety_handle(), output.safety_handle); + output.value = input.value(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(Timestamp input, kortex_driver::Timestamp &output) +{ + output.sec = input.sec(); + output.usec = input.usec(); + + return 0; +} +int ToRosData(UserProfileHandle input, kortex_driver::UserProfileHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(Connection input, kortex_driver::Connection &output) +{ + ToRosData(input.user_handle(), output.user_handle); + output.connection_information = input.connection_information(); + output.connection_identifier = input.connection_identifier(); + + return 0; +} diff --git a/kortex_driver/src/common_ros_converter.h b/kortex_driver/src/common_ros_converter.h new file mode 100644 index 00000000..1ea3e9ad --- /dev/null +++ b/kortex_driver/src/common_ros_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonROS_CONVERTER_H_ +#define _KORTEX_CommonROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_driver/DeviceHandle.h" +#include "kortex_driver/Empty.h" +#include "kortex_driver/NotificationOptions.h" +#include "kortex_driver/SafetyHandle.h" +#include "kortex_driver/NotificationHandle.h" +#include "kortex_driver/SafetyNotification.h" +#include "kortex_driver/Timestamp.h" +#include "kortex_driver/UserProfileHandle.h" +#include "kortex_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToRosData(DeviceHandle input, kortex_driver::DeviceHandle &output); +int ToRosData(Empty input, kortex_driver::Empty &output); +int ToRosData(NotificationOptions input, kortex_driver::NotificationOptions &output); +int ToRosData(SafetyHandle input, kortex_driver::SafetyHandle &output); +int ToRosData(NotificationHandle input, kortex_driver::NotificationHandle &output); +int ToRosData(SafetyNotification input, kortex_driver::SafetyNotification &output); +int ToRosData(Timestamp input, kortex_driver::Timestamp &output); +int ToRosData(UserProfileHandle input, kortex_driver::UserProfileHandle &output); +int ToRosData(Connection input, kortex_driver::Connection &output); + +#endif \ No newline at end of file diff --git a/kortex_driver/src/main.cpp b/kortex_driver/src/main.cpp new file mode 100644 index 00000000..f6b77c4d --- /dev/null +++ b/kortex_driver/src/main.cpp @@ -0,0 +1,261 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +#include +#include + +#define JOINT_COUNT 7 + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "BaseServices"); + + uint32_t cyclic_data_rate = 100; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 2) + { + ROS_INFO("Connecting to IP = %s - node refresh rate = %s", argv[1], argv[2]); + + //Converting the second parameter(the cyclic rate) to an unsigned int variable. + stringstream tempRate; + tempRate << argv[2]; + tempRate >> cyclic_data_rate; + if(tempRate.fail() || tempRate.bad()) + { + ROS_INFO("ERROR - Bad error rate, shutting down the node..."); + ros::shutdown(); + return 0; + } + } + else + { + ROS_INFO("You need to provide an IP adresse as the first parameter and a cycle rate(Hertz) as the second parameter. ex: rosrun package node 192.168.1.1 100"); + ros::shutdown(); + return 0; + } + + BaseServices services_object(argv[1], n); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &BaseServices::SetDeviceID, &services_object); + + ros::ServiceServer serviceRefresh = n.advertiseService("Refresh", &BaseServices::Refresh, &services_object); + ros::ServiceServer serviceRefreshCommand = n.advertiseService("RefreshCommand", &BaseServices::RefreshCommand, &services_object); + ros::ServiceServer serviceRefreshFeedback = n.advertiseService("RefreshFeedback", &BaseServices::RefreshFeedback, &services_object); + ros::ServiceServer serviceRefreshCustomData = n.advertiseService("RefreshCustomData", &BaseServices::RefreshCustomData, &services_object); + ros::ServiceServer serviceCreateUserProfile = n.advertiseService("CreateUserProfile", &BaseServices::CreateUserProfile, &services_object); + ros::ServiceServer serviceUpdateUserProfile = n.advertiseService("UpdateUserProfile", &BaseServices::UpdateUserProfile, &services_object); + ros::ServiceServer serviceReadUserProfile = n.advertiseService("ReadUserProfile", &BaseServices::ReadUserProfile, &services_object); + ros::ServiceServer serviceDeleteUserProfile = n.advertiseService("DeleteUserProfile", &BaseServices::DeleteUserProfile, &services_object); + ros::ServiceServer serviceReadAllUserProfiles = n.advertiseService("ReadAllUserProfiles", &BaseServices::ReadAllUserProfiles, &services_object); + ros::ServiceServer serviceReadAllUsers = n.advertiseService("ReadAllUsers", &BaseServices::ReadAllUsers, &services_object); + ros::ServiceServer serviceChangePassword = n.advertiseService("ChangePassword", &BaseServices::ChangePassword, &services_object); + ros::ServiceServer serviceCreateSequence = n.advertiseService("CreateSequence", &BaseServices::CreateSequence, &services_object); + ros::ServiceServer serviceUpdateSequence = n.advertiseService("UpdateSequence", &BaseServices::UpdateSequence, &services_object); + ros::ServiceServer serviceReadSequence = n.advertiseService("ReadSequence", &BaseServices::ReadSequence, &services_object); + ros::ServiceServer serviceDeleteSequence = n.advertiseService("DeleteSequence", &BaseServices::DeleteSequence, &services_object); + ros::ServiceServer serviceReadAllSequences = n.advertiseService("ReadAllSequences", &BaseServices::ReadAllSequences, &services_object); + ros::ServiceServer serviceDeleteSequenceTask = n.advertiseService("DeleteSequenceTask", &BaseServices::DeleteSequenceTask, &services_object); + ros::ServiceServer serviceDeleteAllSequenceTasks = n.advertiseService("DeleteAllSequenceTasks", &BaseServices::DeleteAllSequenceTasks, &services_object); + ros::ServiceServer servicePlaySequence = n.advertiseService("PlaySequence", &BaseServices::PlaySequence, &services_object); + ros::ServiceServer servicePlayAdvancedSequence = n.advertiseService("PlayAdvancedSequence", &BaseServices::PlayAdvancedSequence, &services_object); + ros::ServiceServer serviceStopSequence = n.advertiseService("StopSequence", &BaseServices::StopSequence, &services_object); + ros::ServiceServer servicePauseSequence = n.advertiseService("PauseSequence", &BaseServices::PauseSequence, &services_object); + ros::ServiceServer serviceResumeSequence = n.advertiseService("ResumeSequence", &BaseServices::ResumeSequence, &services_object); + ros::ServiceServer serviceCreateProtectionZone = n.advertiseService("CreateProtectionZone", &BaseServices::CreateProtectionZone, &services_object); + ros::ServiceServer serviceUpdateProtectionZone = n.advertiseService("UpdateProtectionZone", &BaseServices::UpdateProtectionZone, &services_object); + ros::ServiceServer serviceReadProtectionZone = n.advertiseService("ReadProtectionZone", &BaseServices::ReadProtectionZone, &services_object); + ros::ServiceServer serviceDeleteProtectionZone = n.advertiseService("DeleteProtectionZone", &BaseServices::DeleteProtectionZone, &services_object); + ros::ServiceServer serviceReadAllProtectionZones = n.advertiseService("ReadAllProtectionZones", &BaseServices::ReadAllProtectionZones, &services_object); + ros::ServiceServer serviceCreateMapping = n.advertiseService("CreateMapping", &BaseServices::CreateMapping, &services_object); + ros::ServiceServer serviceReadMapping = n.advertiseService("ReadMapping", &BaseServices::ReadMapping, &services_object); + ros::ServiceServer serviceReadAllMappings = n.advertiseService("ReadAllMappings", &BaseServices::ReadAllMappings, &services_object); + ros::ServiceServer serviceCreateMap = n.advertiseService("CreateMap", &BaseServices::CreateMap, &services_object); + ros::ServiceServer serviceReadAllMaps = n.advertiseService("ReadAllMaps", &BaseServices::ReadAllMaps, &services_object); + ros::ServiceServer serviceActivateMap = n.advertiseService("ActivateMap", &BaseServices::ActivateMap, &services_object); + ros::ServiceServer serviceCreateAction = n.advertiseService("CreateAction", &BaseServices::CreateAction, &services_object); + ros::ServiceServer serviceReadAction = n.advertiseService("ReadAction", &BaseServices::ReadAction, &services_object); + ros::ServiceServer serviceReadAllActions = n.advertiseService("ReadAllActions", &BaseServices::ReadAllActions, &services_object); + ros::ServiceServer serviceDeleteAction = n.advertiseService("DeleteAction", &BaseServices::DeleteAction, &services_object); + ros::ServiceServer serviceUpdateAction = n.advertiseService("UpdateAction", &BaseServices::UpdateAction, &services_object); + ros::ServiceServer serviceExecuteActionFromReference = n.advertiseService("ExecuteActionFromReference", &BaseServices::ExecuteActionFromReference, &services_object); + ros::ServiceServer serviceExecuteAction = n.advertiseService("ExecuteAction", &BaseServices::ExecuteAction, &services_object); + ros::ServiceServer servicePauseAction = n.advertiseService("PauseAction", &BaseServices::PauseAction, &services_object); + ros::ServiceServer serviceStopAction = n.advertiseService("StopAction", &BaseServices::StopAction, &services_object); + ros::ServiceServer serviceResumeAction = n.advertiseService("ResumeAction", &BaseServices::ResumeAction, &services_object); + ros::ServiceServer serviceGetIPv4Configuration = n.advertiseService("GetIPv4Configuration", &BaseServices::GetIPv4Configuration, &services_object); + ros::ServiceServer serviceSetIPv4Configuration = n.advertiseService("SetIPv4Configuration", &BaseServices::SetIPv4Configuration, &services_object); + ros::ServiceServer serviceSetCommunicationInterfaceEnable = n.advertiseService("SetCommunicationInterfaceEnable", &BaseServices::SetCommunicationInterfaceEnable, &services_object); + ros::ServiceServer serviceIsCommunicationInterfaceEnable = n.advertiseService("IsCommunicationInterfaceEnable", &BaseServices::IsCommunicationInterfaceEnable, &services_object); + ros::ServiceServer serviceGetAvailableWifi = n.advertiseService("GetAvailableWifi", &BaseServices::GetAvailableWifi, &services_object); + ros::ServiceServer serviceGetWifiInformation = n.advertiseService("GetWifiInformation", &BaseServices::GetWifiInformation, &services_object); + ros::ServiceServer serviceAddWifiConfiguration = n.advertiseService("AddWifiConfiguration", &BaseServices::AddWifiConfiguration, &services_object); + ros::ServiceServer serviceDeleteWifiConfiguration = n.advertiseService("DeleteWifiConfiguration", &BaseServices::DeleteWifiConfiguration, &services_object); + ros::ServiceServer serviceGetAllConfiguredWifis = n.advertiseService("GetAllConfiguredWifis", &BaseServices::GetAllConfiguredWifis, &services_object); + ros::ServiceServer serviceConnectWifi = n.advertiseService("ConnectWifi", &BaseServices::ConnectWifi, &services_object); + ros::ServiceServer serviceDisconnectWifi = n.advertiseService("DisconnectWifi", &BaseServices::DisconnectWifi, &services_object); + ros::ServiceServer serviceGetConnectedWifiInformation = n.advertiseService("GetConnectedWifiInformation", &BaseServices::GetConnectedWifiInformation, &services_object); + ros::ServiceServer serviceUnsubscribe = n.advertiseService("Unsubscribe", &BaseServices::Unsubscribe, &services_object); + ros::ServiceServer serviceGetFwdKinematics = n.advertiseService("GetFwdKinematics", &BaseServices::GetFwdKinematics, &services_object); + ros::ServiceServer servicePlayCartesianTrajectory = n.advertiseService("PlayCartesianTrajectory", &BaseServices::PlayCartesianTrajectory, &services_object); + ros::ServiceServer servicePlayCartesianTrajectoryPosition = n.advertiseService("PlayCartesianTrajectoryPosition", &BaseServices::PlayCartesianTrajectoryPosition, &services_object); + ros::ServiceServer servicePlayCartesianTrajectoryOrientation = n.advertiseService("PlayCartesianTrajectoryOrientation", &BaseServices::PlayCartesianTrajectoryOrientation, &services_object); + ros::ServiceServer servicePause = n.advertiseService("Pause", &BaseServices::Pause, &services_object); + ros::ServiceServer serviceResume = n.advertiseService("Resume", &BaseServices::Resume, &services_object); + ros::ServiceServer serviceGetMeasuredCartesianPose = n.advertiseService("GetMeasuredCartesianPose", &BaseServices::GetMeasuredCartesianPose, &services_object); + ros::ServiceServer serviceGetCommandedCartesianPose = n.advertiseService("GetCommandedCartesianPose", &BaseServices::GetCommandedCartesianPose, &services_object); + ros::ServiceServer serviceGetTargetedCartesianPose = n.advertiseService("GetTargetedCartesianPose", &BaseServices::GetTargetedCartesianPose, &services_object); + ros::ServiceServer serviceSendTwistCommand = n.advertiseService("SendTwistCommand", &BaseServices::SendTwistCommand, &services_object); + ros::ServiceServer serviceGetMeasuredTwist = n.advertiseService("GetMeasuredTwist", &BaseServices::GetMeasuredTwist, &services_object); + ros::ServiceServer serviceGetCommandedTwist = n.advertiseService("GetCommandedTwist", &BaseServices::GetCommandedTwist, &services_object); + ros::ServiceServer servicePlayJointTrajectory = n.advertiseService("PlayJointTrajectory", &BaseServices::PlayJointTrajectory, &services_object); + ros::ServiceServer servicePlaySelectedJointTrajectory = n.advertiseService("PlaySelectedJointTrajectory", &BaseServices::PlaySelectedJointTrajectory, &services_object); + ros::ServiceServer serviceGetMeasuredJointAngles = n.advertiseService("GetMeasuredJointAngles", &BaseServices::GetMeasuredJointAngles, &services_object); + ros::ServiceServer serviceGetCommandedJointAngles = n.advertiseService("GetCommandedJointAngles", &BaseServices::GetCommandedJointAngles, &services_object); + ros::ServiceServer serviceSendJointSpeedsCommmand = n.advertiseService("SendJointSpeedsCommmand", &BaseServices::SendJointSpeedsCommmand, &services_object); + ros::ServiceServer serviceSendSelectedJointSpeedCommand = n.advertiseService("SendSelectedJointSpeedCommand", &BaseServices::SendSelectedJointSpeedCommand, &services_object); + ros::ServiceServer serviceGetMeasuredJointSpeeds = n.advertiseService("GetMeasuredJointSpeeds", &BaseServices::GetMeasuredJointSpeeds, &services_object); + ros::ServiceServer serviceGetCommandedJointSpeeds = n.advertiseService("GetCommandedJointSpeeds", &BaseServices::GetCommandedJointSpeeds, &services_object); + ros::ServiceServer serviceSendGripperCommand = n.advertiseService("SendGripperCommand", &BaseServices::SendGripperCommand, &services_object); + ros::ServiceServer serviceGetMeasuredGripperMovement = n.advertiseService("GetMeasuredGripperMovement", &BaseServices::GetMeasuredGripperMovement, &services_object); + ros::ServiceServer serviceGetCommandedGripperMovement = n.advertiseService("GetCommandedGripperMovement", &BaseServices::GetCommandedGripperMovement, &services_object); + ros::ServiceServer serviceSetAdmittance = n.advertiseService("SetAdmittance", &BaseServices::SetAdmittance, &services_object); + ros::ServiceServer serviceSetTwistWrenchReferenceFrame = n.advertiseService("SetTwistWrenchReferenceFrame", &BaseServices::SetTwistWrenchReferenceFrame, &services_object); + ros::ServiceServer serviceSetOperatingMode = n.advertiseService("SetOperatingMode", &BaseServices::SetOperatingMode, &services_object); + ros::ServiceServer serviceApplyEmergencyStop = n.advertiseService("ApplyEmergencyStop", &BaseServices::ApplyEmergencyStop, &services_object); + ros::ServiceServer serviceClearFaults = n.advertiseService("ClearFaults", &BaseServices::ClearFaults, &services_object); + ros::ServiceServer serviceGetActiveMap = n.advertiseService("GetActiveMap", &BaseServices::GetActiveMap, &services_object); + ros::ServiceServer serviceGetControlMode = n.advertiseService("GetControlMode", &BaseServices::GetControlMode, &services_object); + ros::ServiceServer serviceGetOperatingMode = n.advertiseService("GetOperatingMode", &BaseServices::GetOperatingMode, &services_object); + ros::ServiceServer serviceSetServoingMode = n.advertiseService("SetServoingMode", &BaseServices::SetServoingMode, &services_object); + ros::ServiceServer serviceGetServoingMode = n.advertiseService("GetServoingMode", &BaseServices::GetServoingMode, &services_object); + ros::ServiceServer serviceGetSequenceState = n.advertiseService("GetSequenceState", &BaseServices::GetSequenceState, &services_object); + ros::ServiceServer serviceGetProtectionZoneState = n.advertiseService("GetProtectionZoneState", &BaseServices::GetProtectionZoneState, &services_object); + ros::ServiceServer serviceGetActionExecutionState = n.advertiseService("GetActionExecutionState", &BaseServices::GetActionExecutionState, &services_object); + ros::ServiceServer serviceRestoreFactorySettings = n.advertiseService("RestoreFactorySettings", &BaseServices::RestoreFactorySettings, &services_object); + ros::ServiceServer serviceRestoreNetworkFactorySettings = n.advertiseService("RestoreNetworkFactorySettings", &BaseServices::RestoreNetworkFactorySettings, &services_object); + ros::ServiceServer serviceReboot = n.advertiseService("Reboot", &BaseServices::Reboot, &services_object); + ros::ServiceServer serviceGetAllConnectedControllers = n.advertiseService("GetAllConnectedControllers", &BaseServices::GetAllConnectedControllers, &services_object); + ros::ServiceServer serviceGetControllerState = n.advertiseService("GetControllerState", &BaseServices::GetControllerState, &services_object); + ros::ServiceServer serviceGetActuatorCount = n.advertiseService("GetActuatorCount", &BaseServices::GetActuatorCount, &services_object); + ros::ServiceServer serviceStartWifiScan = n.advertiseService("StartWifiScan", &BaseServices::StartWifiScan, &services_object); + ros::ServiceServer serviceGetConfiguredWifi = n.advertiseService("GetConfiguredWifi", &BaseServices::GetConfiguredWifi, &services_object); + ros::ServiceServer serviceGetArmState = n.advertiseService("GetArmState", &BaseServices::GetArmState, &services_object); + ros::ServiceServer serviceGetIPv4Information = n.advertiseService("GetIPv4Information", &BaseServices::GetIPv4Information, &services_object); + + + ROS_INFO("Node's services initialized correctly."); + + ros::Publisher pub_base_feedback = n.advertise("base_feedback", 1000); + ros::Publisher pub_joint_state = n.advertise("base_feedback/joint_state", 1000); + + kortex_driver::Feedback base_feedback; + kortex_driver::RefreshFeedback::Request req; + kortex_driver::RefreshFeedback::Response res; + + sensor_msgs::JointState joint_state; + + ros::Rate rate(cyclic_data_rate); // 100 hz + while (!ros::isShuttingDown()) + { + services_object.RefreshFeedback(req, res); + + base_feedback.frame_id = res.output.frame_id; + + base_feedback.base.arm_voltage = res.output.base.arm_voltage; + base_feedback.base.arm_current = res.output.base.arm_current; + base_feedback.base.temperature_cpu = res.output.base.temperature_cpu; + base_feedback.base.temperature_ambient = res.output.base.temperature_ambient; + base_feedback.base.imu_acceleration_x = res.output.base.imu_acceleration_x; + base_feedback.base.imu_acceleration_y = res.output.base.imu_acceleration_y; + base_feedback.base.imu_acceleration_z = res.output.base.imu_acceleration_z; + base_feedback.base.imu_angular_velocity_x = res.output.base.imu_angular_velocity_x; + base_feedback.base.imu_angular_velocity_y = res.output.base.imu_angular_velocity_y; + base_feedback.base.imu_angular_velocity_z = res.output.base.imu_angular_velocity_z; + base_feedback.base.tool_pose_x = res.output.base.tool_pose_x; + base_feedback.base.tool_pose_y = res.output.base.tool_pose_y; + base_feedback.base.tool_pose_z = res.output.base.tool_pose_z; + base_feedback.base.tool_pose_theta_x = res.output.base.tool_pose_theta_x; + base_feedback.base.tool_pose_theta_y = res.output.base.tool_pose_theta_y; + base_feedback.base.tool_pose_theta_z = res.output.base.tool_pose_theta_z; + base_feedback.base.tool_external_wrench_force_x = res.output.base.tool_external_wrench_force_x; + base_feedback.base.tool_external_wrench_force_y = res.output.base.tool_external_wrench_force_y; + base_feedback.base.tool_external_wrench_force_z = res.output.base.tool_external_wrench_force_z; + base_feedback.base.tool_external_wrench_torque_x = res.output.base.tool_external_wrench_torque_x; + base_feedback.base.tool_external_wrench_torque_y = res.output.base.tool_external_wrench_torque_y; + base_feedback.base.tool_external_wrench_torque_z = res.output.base.tool_external_wrench_torque_z; + base_feedback.base.fault_bank_a = res.output.base.fault_bank_a; + base_feedback.base.fault_bank_b = res.output.base.fault_bank_b; + base_feedback.base.warning_bank_a = res.output.base.warning_bank_a; + base_feedback.base.warning_bank_b = res.output.base.warning_bank_b; + + base_feedback.actuators.clear(); + + joint_state.position.resize(JOINT_COUNT); + joint_state.velocity.resize(JOINT_COUNT); + joint_state.effort.resize(JOINT_COUNT); + joint_state.name.resize(JOINT_COUNT); + + for(int i = 0; i < JOINT_COUNT; i++) + { + kortex_driver::ActuatorFeedback temp; + + temp.status_flags = res.output.actuators[i].status_flags; + temp.jitter_comm = res.output.actuators[i].jitter_comm; + temp.position = res.output.actuators[i].position; + temp.velocity = res.output.actuators[i].velocity; + temp.torque = res.output.actuators[i].torque; + temp.current_motor = res.output.actuators[i].current_motor; + temp.voltage = res.output.actuators[i].voltage; + temp.temperature_motor = res.output.actuators[i].temperature_motor; + temp.temperature_core = res.output.actuators[i].temperature_core; + temp.fault_bank_a = res.output.actuators[i].fault_bank_a; + temp.fault_bank_b = res.output.actuators[i].fault_bank_b; + temp.warning_bank_a = res.output.actuators[i].warning_bank_a; + temp.warning_bank_b = res.output.actuators[i].warning_bank_b; + + base_feedback.actuators.push_back(temp); + + joint_state.name[i] = "Actuator" + std::to_string(i + 1); + joint_state.position[i] = TO_RAD(res.output.actuators[i].position); + joint_state.velocity[i] = TO_RAD(res.output.actuators[i].velocity); + joint_state.effort[i] = res.output.actuators[i].torque; + } + base_feedback.interconnect.position = res.output.interconnect.position; + + + + joint_state.header.stamp = ros::Time::now(); + joint_state.header.frame_id = std::to_string(res.output.frame_id); + + pub_base_feedback.publish(base_feedback); + pub_joint_state.publish(joint_state); + + + ros::spinOnce(); + + + + rate.sleep(); + } + + return 1; +} \ No newline at end of file diff --git a/kortex_driver/src/node.cpp b/kortex_driver/src/node.cpp new file mode 100644 index 00000000..b2c31a73 --- /dev/null +++ b/kortex_driver/src/node.cpp @@ -0,0 +1,3945 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "common_ros_converter.h" +#include "common_proto_converter.h" +#include "basecyclic_ros_converter.h" +#include "basecyclic_proto_converter.h" + +#include "base_ros_converter.h" +#include "base_proto_converter.h" +BaseServices::BaseServices(char* ip, ros::NodeHandle& n) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = 0; + m_apiOptions.timeout_ms = 3000; + + m_basecyclic = new BaseCyclic::BaseCyclicClient(m_router); + m_base = new Base::BaseClient(m_router);m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + std::cout << "\nSession Created\n"; + + m_pub_Error = m_n.advertise("KortexError", 1000); + m_pub_ConfigurationChangeTopic = m_n.advertise("ConfigurationChangeTopic", 1000); + m_pub_MappingInfoTopic = m_n.advertise("MappingInfoTopic", 1000); + m_pub_ControlModeTopic = m_n.advertise("ControlModeTopic", 1000); + m_pub_OperatingModeTopic = m_n.advertise("OperatingModeTopic", 1000); + m_pub_SequenceInfoTopic = m_n.advertise("SequenceInfoTopic", 1000); + m_pub_ProtectionZoneTopic = m_n.advertise("ProtectionZoneTopic", 1000); + m_pub_UserTopic = m_n.advertise("UserTopic", 1000); + m_pub_ControllerTopic = m_n.advertise("ControllerTopic", 1000); + m_pub_ActionTopic = m_n.advertise("ActionTopic", 1000); + m_pub_RobotEventTopic = m_n.advertise("RobotEventTopic", 1000); + m_pub_ServoingModeTopic = m_n.advertise("ServoingModeTopic", 1000); + m_pub_FactoryTopic = m_n.advertise("FactoryTopic", 1000); + m_pub_NetworkTopic = m_n.advertise("NetworkTopic", 1000); + m_pub_ArmStateTopic = m_n.advertise("ArmStateTopic", 1000);std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool BaseServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + m_CurrentDeviceID = req.device_id; + + return true; +} + +bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} + + + + +bool BaseServices::Refresh(kortex_driver::Refresh::Request &req, kortex_driver::Refresh::Response &res) +{ + Command input; + ToProtoData(req.input, &input); + Feedback output; + kortex_driver::KortexError result_error; + + try + { + output = m_basecyclic->Refresh(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::RefreshCommand(kortex_driver::RefreshCommand::Request &req, kortex_driver::RefreshCommand::Response &res) +{ + Command input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_basecyclic->RefreshCommand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::RefreshFeedback(kortex_driver::RefreshFeedback::Request &req, kortex_driver::RefreshFeedback::Response &res) +{ + Empty input; + Feedback output; + kortex_driver::KortexError result_error; + + try + { + output = m_basecyclic->RefreshFeedback(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::RefreshCustomData(kortex_driver::RefreshCustomData::Request &req, kortex_driver::RefreshCustomData::Response &res) +{ + CustomData input; + ToProtoData(req.input, &input); + CustomData output; + kortex_driver::KortexError result_error; + + try + { + output = m_basecyclic->RefreshCustomData(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + + + +bool BaseServices::CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) +{ + FullUserProfile input; + ToProtoData(req.input, &input); + UserProfileHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateUserProfile(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) +{ + UserProfile input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateUserProfile(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) +{ + UserProfileHandle input; + ToProtoData(req.input, &input); + UserProfile output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadUserProfile(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) +{ + UserProfileHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteUserProfile(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) +{ + Empty input; + UserProfileList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllUserProfiles(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) +{ + Empty input; + UserList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllUsers(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) +{ + PasswordChange input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ChangePassword(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) +{ + Sequence input; + ToProtoData(req.input, &input); + SequenceHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateSequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) +{ + Sequence input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateSequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) +{ + SequenceHandle input; + ToProtoData(req.input, &input); + Sequence output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadSequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) +{ + SequenceHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteSequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) +{ + Empty input; + SequenceList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllSequences(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) +{ + SequenceTaskHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteSequenceTask(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) +{ + SequenceHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteAllSequenceTasks(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) +{ + SequenceHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlaySequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) +{ + AdvancedSequenceHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlayAdvancedSequence(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->StopSequence(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PauseSequence(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ResumeSequence(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) +{ + ProtectionZone input; + ToProtoData(req.input, &input); + ProtectionZoneHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateProtectionZone(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) +{ + ProtectionZone input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateProtectionZone(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) +{ + ProtectionZoneHandle input; + ToProtoData(req.input, &input); + ProtectionZone output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadProtectionZone(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) +{ + ProtectionZoneHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteProtectionZone(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) +{ + Empty input; + ProtectionZoneList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllProtectionZones(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) +{ + Mapping input; + ToProtoData(req.input, &input); + MappingHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateMapping(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) +{ + MappingHandle input; + ToProtoData(req.input, &input); + Mapping output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadMapping(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) +{ + Empty input; + MappingList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllMappings(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) +{ + Map input; + ToProtoData(req.input, &input); + MapHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateMap(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) +{ + MappingHandle input; + ToProtoData(req.input, &input); + MapList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllMaps(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) +{ + ActivateMapHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ActivateMap(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) +{ + Action input; + ToProtoData(req.input, &input); + ActionHandle output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->CreateAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) +{ + ActionHandle input; + ToProtoData(req.input, &input); + Action output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) +{ + RequestedActionType input; + ToProtoData(req.input, &input); + ActionList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->ReadAllActions(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) +{ + ActionHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) +{ + Action input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->UpdateAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) +{ + ActionHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ExecuteActionFromReference(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) +{ + Action input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ExecuteAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PauseAction(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->StopAction(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ResumeAction(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) +{ + NetworkHandle input; + ToProtoData(req.input, &input); + IPv4Configuration output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetIPv4Configuration(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) +{ + FullIPv4Configuration input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetIPv4Configuration(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) +{ + CommunicationInterfaceConfiguration input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetCommunicationInterfaceEnable(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) +{ + NetworkHandle input; + ToProtoData(req.input, &input); + CommunicationInterfaceConfiguration output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->IsCommunicationInterfaceEnable(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) +{ + Empty input; + WifiInformationList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetAvailableWifi(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) +{ + Ssid input; + ToProtoData(req.input, &input); + WifiInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetWifiInformation(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) +{ + WifiConfiguration input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->AddWifiConfiguration(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) +{ + Ssid input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DeleteWifiConfiguration(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) +{ + Empty input; + WifiConfigurationList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetAllConfiguredWifis(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) +{ + Ssid input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ConnectWifi(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->DisconnectWifi(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) +{ + Empty input; + WifiInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetConnectedWifiInformation(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::Unsubscribe(kortex_driver::Unsubscribe::Request &req, kortex_driver::Unsubscribe::Response &res) +{ + NotificationHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->Unsubscribe(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::OnNotificationConfigurationChangeTopic(kortex_driver::ConfigurationChangeTopic::Request &req, kortex_driver::ConfigurationChangeTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ConfigurationChangeNotification) > callback = std::bind(&BaseServices::cb_ConfigurationChangeTopic, this, std::placeholders::_1); + output = m_base->OnNotificationConfigurationChangeTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ConfigurationChangeTopic(Base::ConfigurationChangeNotification notif) +{ + kortex_driver::ConfigurationChangeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ConfigurationChangeTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationMappingInfoTopic(kortex_driver::MappingInfoTopic::Request &req, kortex_driver::MappingInfoTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::MappingInfoNotification) > callback = std::bind(&BaseServices::cb_MappingInfoTopic, this, std::placeholders::_1); + output = m_base->OnNotificationMappingInfoTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_MappingInfoTopic(Base::MappingInfoNotification notif) +{ + kortex_driver::MappingInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_MappingInfoTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationControlModeTopic(kortex_driver::ControlModeTopic::Request &req, kortex_driver::ControlModeTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ControlModeNotification) > callback = std::bind(&BaseServices::cb_ControlModeTopic, this, std::placeholders::_1); + output = m_base->OnNotificationControlModeTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ControlModeTopic(Base::ControlModeNotification notif) +{ + kortex_driver::ControlModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlModeTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationOperatingModeTopic(kortex_driver::OperatingModeTopic::Request &req, kortex_driver::OperatingModeTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::OperatingModeNotification) > callback = std::bind(&BaseServices::cb_OperatingModeTopic, this, std::placeholders::_1); + output = m_base->OnNotificationOperatingModeTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_OperatingModeTopic(Base::OperatingModeNotification notif) +{ + kortex_driver::OperatingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_OperatingModeTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationSequenceInfoTopic(kortex_driver::SequenceInfoTopic::Request &req, kortex_driver::SequenceInfoTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::SequenceInfoNotification) > callback = std::bind(&BaseServices::cb_SequenceInfoTopic, this, std::placeholders::_1); + output = m_base->OnNotificationSequenceInfoTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_SequenceInfoTopic(Base::SequenceInfoNotification notif) +{ + kortex_driver::SequenceInfoNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_SequenceInfoTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationProtectionZoneTopic(kortex_driver::ProtectionZoneTopic::Request &req, kortex_driver::ProtectionZoneTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ProtectionZoneNotification) > callback = std::bind(&BaseServices::cb_ProtectionZoneTopic, this, std::placeholders::_1); + output = m_base->OnNotificationProtectionZoneTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ProtectionZoneTopic(Base::ProtectionZoneNotification notif) +{ + kortex_driver::ProtectionZoneNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ProtectionZoneTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationUserTopic(kortex_driver::UserTopic::Request &req, kortex_driver::UserTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::UserNotification) > callback = std::bind(&BaseServices::cb_UserTopic, this, std::placeholders::_1); + output = m_base->OnNotificationUserTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_UserTopic(Base::UserNotification notif) +{ + kortex_driver::UserNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_UserTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationControllerTopic(kortex_driver::ControllerTopic::Request &req, kortex_driver::ControllerTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ControllerNotification) > callback = std::bind(&BaseServices::cb_ControllerTopic, this, std::placeholders::_1); + output = m_base->OnNotificationControllerTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ControllerTopic(Base::ControllerNotification notif) +{ + kortex_driver::ControllerNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControllerTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationActionTopic(kortex_driver::ActionTopic::Request &req, kortex_driver::ActionTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ActionNotification) > callback = std::bind(&BaseServices::cb_ActionTopic, this, std::placeholders::_1); + output = m_base->OnNotificationActionTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ActionTopic(Base::ActionNotification notif) +{ + kortex_driver::ActionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ActionTopic.publish(ros_msg); +} + +bool BaseServices::OnNotificationRobotEventTopic(kortex_driver::RobotEventTopic::Request &req, kortex_driver::RobotEventTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::RobotEventNotification) > callback = std::bind(&BaseServices::cb_RobotEventTopic, this, std::placeholders::_1); + output = m_base->OnNotificationRobotEventTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_RobotEventTopic(Base::RobotEventNotification notif) +{ + kortex_driver::RobotEventNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_RobotEventTopic.publish(ros_msg); +} + +bool BaseServices::GetFwdKinematics(kortex_driver::GetFwdKinematics::Request &req, kortex_driver::GetFwdKinematics::Response &res) +{ + Empty input; + TransformationMatrix output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetFwdKinematics(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) +{ + ConstrainedPose input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlayCartesianTrajectory(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) +{ + ConstrainedPosition input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlayCartesianTrajectoryPosition(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) +{ + ConstrainedOrientation input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlayCartesianTrajectoryOrientation(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::Pause(kortex_driver::Pause::Request &req, kortex_driver::Pause::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->Pause(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::Resume(kortex_driver::Resume::Request &req, kortex_driver::Resume::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->Resume(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) +{ + Empty input; + Pose output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetMeasuredCartesianPose(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetCommandedCartesianPose(kortex_driver::GetCommandedCartesianPose::Request &req, kortex_driver::GetCommandedCartesianPose::Response &res) +{ + Empty input; + Pose output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetCommandedCartesianPose(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetTargetedCartesianPose(kortex_driver::GetTargetedCartesianPose::Request &req, kortex_driver::GetTargetedCartesianPose::Response &res) +{ + Empty input; + Pose output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetTargetedCartesianPose(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) +{ + TwistCommand input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SendTwistCommand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetMeasuredTwist(kortex_driver::GetMeasuredTwist::Request &req, kortex_driver::GetMeasuredTwist::Response &res) +{ + Empty input; + Twist output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetMeasuredTwist(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetCommandedTwist(kortex_driver::GetCommandedTwist::Request &req, kortex_driver::GetCommandedTwist::Response &res) +{ + Empty input; + Twist output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetCommandedTwist(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) +{ + ConstrainedJointAngles input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlayJointTrajectory(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) +{ + ConstrainedJointAngle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->PlaySelectedJointTrajectory(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) +{ + Empty input; + JointAngles output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetMeasuredJointAngles(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetCommandedJointAngles(kortex_driver::GetCommandedJointAngles::Request &req, kortex_driver::GetCommandedJointAngles::Response &res) +{ + Empty input; + JointAngles output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetCommandedJointAngles(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SendJointSpeedsCommmand(kortex_driver::SendJointSpeedsCommmand::Request &req, kortex_driver::SendJointSpeedsCommmand::Response &res) +{ + JointSpeeds input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SendJointSpeedsCommmand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) +{ + JointSpeed input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SendSelectedJointSpeedCommand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetMeasuredJointSpeeds(kortex_driver::GetMeasuredJointSpeeds::Request &req, kortex_driver::GetMeasuredJointSpeeds::Response &res) +{ + Empty input; + JointSpeeds output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetMeasuredJointSpeeds(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetCommandedJointSpeeds(kortex_driver::GetCommandedJointSpeeds::Request &req, kortex_driver::GetCommandedJointSpeeds::Response &res) +{ + Empty input; + JointSpeeds output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetCommandedJointSpeeds(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) +{ + GripperCommand input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SendGripperCommand(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) +{ + GripperRequest input; + ToProtoData(req.input, &input); + Gripper output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetMeasuredGripperMovement(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetCommandedGripperMovement(kortex_driver::GetCommandedGripperMovement::Request &req, kortex_driver::GetCommandedGripperMovement::Response &res) +{ + GripperRequest input; + ToProtoData(req.input, &input); + Gripper output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetCommandedGripperMovement(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) +{ + Admittance input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetAdmittance(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::SetTwistWrenchReferenceFrame(kortex_driver::SetTwistWrenchReferenceFrame::Request &req, kortex_driver::SetTwistWrenchReferenceFrame::Response &res) +{ + CartesianReferenceFrameRequest input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetTwistWrenchReferenceFrame(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) +{ + OperatingModeInformation input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetOperatingMode(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ApplyEmergencyStop(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::ClearFaults(kortex_driver::ClearFaults::Request &req, kortex_driver::ClearFaults::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->ClearFaults(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetActiveMap(kortex_driver::GetActiveMap::Request &req, kortex_driver::GetActiveMap::Response &res) +{ + MappingHandle input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->GetActiveMap(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetControlMode(kortex_driver::GetControlMode::Request &req, kortex_driver::GetControlMode::Response &res) +{ + Empty input; + ControlModeInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetControlMode(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) +{ + Empty input; + OperatingModeInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetOperatingMode(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) +{ + ServoingModeInformation input; + ToProtoData(req.input, &input); + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->SetServoingMode(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) +{ + Empty input; + ServoingModeInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetServoingMode(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::OnNotificationServoingModeTopic(kortex_driver::ServoingModeTopic::Request &req, kortex_driver::ServoingModeTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ServoingModeNotification) > callback = std::bind(&BaseServices::cb_ServoingModeTopic, this, std::placeholders::_1); + output = m_base->OnNotificationServoingModeTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ServoingModeTopic(Base::ServoingModeNotification notif) +{ + kortex_driver::ServoingModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ServoingModeTopic.publish(ros_msg); +} + +bool BaseServices::GetSequenceState(kortex_driver::GetSequenceState::Request &req, kortex_driver::GetSequenceState::Response &res) +{ + SequenceHandle input; + ToProtoData(req.input, &input); + SequenceInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetSequenceState(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetProtectionZoneState(kortex_driver::GetProtectionZoneState::Request &req, kortex_driver::GetProtectionZoneState::Response &res) +{ + ProtectionZoneHandle input; + ToProtoData(req.input, &input); + ProtectionZoneInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetProtectionZoneState(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetActionExecutionState(kortex_driver::GetActionExecutionState::Request &req, kortex_driver::GetActionExecutionState::Response &res) +{ + Empty input; + ActionExecutionState output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetActionExecutionState(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->RestoreFactorySettings(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::RestoreNetworkFactorySettings(kortex_driver::RestoreNetworkFactorySettings::Request &req, kortex_driver::RestoreNetworkFactorySettings::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->RestoreNetworkFactorySettings(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::Reboot(kortex_driver::Reboot::Request &req, kortex_driver::Reboot::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->Reboot(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::OnNotificationFactoryTopic(kortex_driver::FactoryTopic::Request &req, kortex_driver::FactoryTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::FactoryNotification) > callback = std::bind(&BaseServices::cb_FactoryTopic, this, std::placeholders::_1); + output = m_base->OnNotificationFactoryTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_FactoryTopic(Base::FactoryNotification notif) +{ + kortex_driver::FactoryNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_FactoryTopic.publish(ros_msg); +} + +bool BaseServices::GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) +{ + Empty input; + ControllerList output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetAllConnectedControllers(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) +{ + ControllerHandle input; + ToProtoData(req.input, &input); + ControllerState output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetControllerState(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) +{ + Empty input; + ActuatorInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetActuatorCount(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) +{ + Empty input; + Empty output; + kortex_driver::KortexError result_error; + + try + { + m_base->StartWifiScan(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool BaseServices::GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) +{ + Ssid input; + ToProtoData(req.input, &input); + WifiConfiguration output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetConfiguredWifi(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::OnNotificationNetworkTopic(kortex_driver::NetworkTopic::Request &req, kortex_driver::NetworkTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::NetworkNotification) > callback = std::bind(&BaseServices::cb_NetworkTopic, this, std::placeholders::_1); + output = m_base->OnNotificationNetworkTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_NetworkTopic(Base::NetworkNotification notif) +{ + kortex_driver::NetworkNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_NetworkTopic.publish(ros_msg); +} + +bool BaseServices::GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) +{ + Empty input; + ArmStateInformation output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetArmState(m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseServices::OnNotificationArmStateTopic(kortex_driver::ArmStateTopic::Request &req, kortex_driver::ArmStateTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_driver::KortexError result_error; + + try + { + std::function< void (Base::ArmStateNotification) > callback = std::bind(&BaseServices::cb_ArmStateTopic, this, std::placeholders::_1); + output = m_base->OnNotificationArmStateTopic(callback, input); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void BaseServices::cb_ArmStateTopic(Base::ArmStateNotification notif) +{ + kortex_driver::ArmStateNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ArmStateTopic.publish(ros_msg); +} + +bool BaseServices::GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) +{ + NetworkHandle input; + ToProtoData(req.input, &input); + IPv4Information output; + kortex_driver::KortexError result_error; + + try + { + output = m_base->GetIPv4Information(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_driver/src/node.h b/kortex_driver/src/node.h new file mode 100644 index 00000000..6de66a03 --- /dev/null +++ b/kortex_driver/src/node.h @@ -0,0 +1,345 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_SERVICES_H_ +#define _KORTEX_SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include "kortex_driver/Refresh.h" +#include "kortex_driver/RefreshCommand.h" +#include "kortex_driver/RefreshFeedback.h" +#include "kortex_driver/RefreshCustomData.h" +#include "kortex_driver/CreateUserProfile.h" +#include "kortex_driver/UpdateUserProfile.h" +#include "kortex_driver/ReadUserProfile.h" +#include "kortex_driver/DeleteUserProfile.h" +#include "kortex_driver/ReadAllUserProfiles.h" +#include "kortex_driver/ReadAllUsers.h" +#include "kortex_driver/ChangePassword.h" +#include "kortex_driver/CreateSequence.h" +#include "kortex_driver/UpdateSequence.h" +#include "kortex_driver/ReadSequence.h" +#include "kortex_driver/DeleteSequence.h" +#include "kortex_driver/ReadAllSequences.h" +#include "kortex_driver/DeleteSequenceTask.h" +#include "kortex_driver/DeleteAllSequenceTasks.h" +#include "kortex_driver/PlaySequence.h" +#include "kortex_driver/PlayAdvancedSequence.h" +#include "kortex_driver/StopSequence.h" +#include "kortex_driver/PauseSequence.h" +#include "kortex_driver/ResumeSequence.h" +#include "kortex_driver/CreateProtectionZone.h" +#include "kortex_driver/UpdateProtectionZone.h" +#include "kortex_driver/ReadProtectionZone.h" +#include "kortex_driver/DeleteProtectionZone.h" +#include "kortex_driver/ReadAllProtectionZones.h" +#include "kortex_driver/CreateMapping.h" +#include "kortex_driver/ReadMapping.h" +#include "kortex_driver/ReadAllMappings.h" +#include "kortex_driver/CreateMap.h" +#include "kortex_driver/ReadAllMaps.h" +#include "kortex_driver/ActivateMap.h" +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" +#include "kortex_driver/GetIPv4Configuration.h" +#include "kortex_driver/SetIPv4Configuration.h" +#include "kortex_driver/SetCommunicationInterfaceEnable.h" +#include "kortex_driver/IsCommunicationInterfaceEnable.h" +#include "kortex_driver/GetAvailableWifi.h" +#include "kortex_driver/GetWifiInformation.h" +#include "kortex_driver/AddWifiConfiguration.h" +#include "kortex_driver/DeleteWifiConfiguration.h" +#include "kortex_driver/GetAllConfiguredWifis.h" +#include "kortex_driver/ConnectWifi.h" +#include "kortex_driver/DisconnectWifi.h" +#include "kortex_driver/GetConnectedWifiInformation.h" +#include "kortex_driver/Unsubscribe.h" +#include "kortex_driver/ConfigurationChangeTopic.h" +#include "kortex_driver/MappingInfoTopic.h" +#include "kortex_driver/ControlModeTopic.h" +#include "kortex_driver/OperatingModeTopic.h" +#include "kortex_driver/SequenceInfoTopic.h" +#include "kortex_driver/ProtectionZoneTopic.h" +#include "kortex_driver/UserTopic.h" +#include "kortex_driver/ControllerTopic.h" +#include "kortex_driver/ActionTopic.h" +#include "kortex_driver/RobotEventTopic.h" +#include "kortex_driver/GetFwdKinematics.h" +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/PlayCartesianTrajectoryPosition.h" +#include "kortex_driver/PlayCartesianTrajectoryOrientation.h" +#include "kortex_driver/Pause.h" +#include "kortex_driver/Resume.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/GetCommandedCartesianPose.h" +#include "kortex_driver/GetTargetedCartesianPose.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/GetMeasuredTwist.h" +#include "kortex_driver/GetCommandedTwist.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/PlaySelectedJointTrajectory.h" +#include "kortex_driver/GetMeasuredJointAngles.h" +#include "kortex_driver/GetCommandedJointAngles.h" +#include "kortex_driver/SendJointSpeedsCommmand.h" +#include "kortex_driver/SendSelectedJointSpeedCommand.h" +#include "kortex_driver/GetMeasuredJointSpeeds.h" +#include "kortex_driver/GetCommandedJointSpeeds.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/GetMeasuredGripperMovement.h" +#include "kortex_driver/GetCommandedGripperMovement.h" +#include "kortex_driver/SetAdmittance.h" +#include "kortex_driver/SetTwistWrenchReferenceFrame.h" +#include "kortex_driver/SetOperatingMode.h" +#include "kortex_driver/ApplyEmergencyStop.h" +#include "kortex_driver/ClearFaults.h" +#include "kortex_driver/GetActiveMap.h" +#include "kortex_driver/GetControlMode.h" +#include "kortex_driver/GetOperatingMode.h" +#include "kortex_driver/SetServoingMode.h" +#include "kortex_driver/GetServoingMode.h" +#include "kortex_driver/ServoingModeTopic.h" +#include "kortex_driver/GetSequenceState.h" +#include "kortex_driver/GetProtectionZoneState.h" +#include "kortex_driver/GetActionExecutionState.h" +#include "kortex_driver/RestoreFactorySettings.h" +#include "kortex_driver/RestoreNetworkFactorySettings.h" +#include "kortex_driver/Reboot.h" +#include "kortex_driver/FactoryTopic.h" +#include "kortex_driver/GetAllConnectedControllers.h" +#include "kortex_driver/GetControllerState.h" +#include "kortex_driver/GetActuatorCount.h" +#include "kortex_driver/StartWifiScan.h" +#include "kortex_driver/GetConfiguredWifi.h" +#include "kortex_driver/NetworkTopic.h" +#include "kortex_driver/GetArmState.h" +#include "kortex_driver/ArmStateTopic.h" +#include "kortex_driver/GetIPv4Information.h" +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" + +#include "kortex_driver/ApiOptions.h" + +using namespace std; +using namespace Kinova::Api; +using namespace Kinova::Api::Common; +using namespace Kinova::Api::BaseCyclic; +using namespace Kinova::Api; +using namespace Kinova::Api::Base; + +class BaseServices +{ + public: + BaseServices(char* ip, ros::NodeHandle& n); + bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); + + + bool Refresh(kortex_driver::Refresh::Request &req, kortex_driver::Refresh::Response &res); + bool RefreshCommand(kortex_driver::RefreshCommand::Request &req, kortex_driver::RefreshCommand::Response &res); + bool RefreshFeedback(kortex_driver::RefreshFeedback::Request &req, kortex_driver::RefreshFeedback::Response &res); + bool RefreshCustomData(kortex_driver::RefreshCustomData::Request &req, kortex_driver::RefreshCustomData::Response &res); + + + bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res); + bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res); + bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res); + bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res); + bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res); + bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res); + bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res); + bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res); + bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res); + bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res); + bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res); + bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res); + bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res); + bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res); + bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res); + bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res); + bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res); + bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res); + bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res); + bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res); + bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res); + bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res); + bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res); + bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res); + bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res); + bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res); + bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res); + bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res); + bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res); + bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res); + bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res); + bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res); + bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res); + bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res); + bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res); + bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res); + bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res); + bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res); + bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res); + bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res); + bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res); + bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res); + bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res); + bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res); + bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res); + bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res); + bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res); + bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res); + bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res); + bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res); + bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res); + bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res); + bool Unsubscribe(kortex_driver::Unsubscribe::Request &req, kortex_driver::Unsubscribe::Response &res); + bool OnNotificationConfigurationChangeTopic(kortex_driver::ConfigurationChangeTopic::Request &req, kortex_driver::ConfigurationChangeTopic::Response &res); + void cb_ConfigurationChangeTopic(ConfigurationChangeNotification notif); + bool OnNotificationMappingInfoTopic(kortex_driver::MappingInfoTopic::Request &req, kortex_driver::MappingInfoTopic::Response &res); + void cb_MappingInfoTopic(MappingInfoNotification notif); + bool OnNotificationControlModeTopic(kortex_driver::ControlModeTopic::Request &req, kortex_driver::ControlModeTopic::Response &res); + void cb_ControlModeTopic(ControlModeNotification notif); + bool OnNotificationOperatingModeTopic(kortex_driver::OperatingModeTopic::Request &req, kortex_driver::OperatingModeTopic::Response &res); + void cb_OperatingModeTopic(OperatingModeNotification notif); + bool OnNotificationSequenceInfoTopic(kortex_driver::SequenceInfoTopic::Request &req, kortex_driver::SequenceInfoTopic::Response &res); + void cb_SequenceInfoTopic(SequenceInfoNotification notif); + bool OnNotificationProtectionZoneTopic(kortex_driver::ProtectionZoneTopic::Request &req, kortex_driver::ProtectionZoneTopic::Response &res); + void cb_ProtectionZoneTopic(ProtectionZoneNotification notif); + bool OnNotificationUserTopic(kortex_driver::UserTopic::Request &req, kortex_driver::UserTopic::Response &res); + void cb_UserTopic(UserNotification notif); + bool OnNotificationControllerTopic(kortex_driver::ControllerTopic::Request &req, kortex_driver::ControllerTopic::Response &res); + void cb_ControllerTopic(ControllerNotification notif); + bool OnNotificationActionTopic(kortex_driver::ActionTopic::Request &req, kortex_driver::ActionTopic::Response &res); + void cb_ActionTopic(ActionNotification notif); + bool OnNotificationRobotEventTopic(kortex_driver::RobotEventTopic::Request &req, kortex_driver::RobotEventTopic::Response &res); + void cb_RobotEventTopic(RobotEventNotification notif); + bool GetFwdKinematics(kortex_driver::GetFwdKinematics::Request &req, kortex_driver::GetFwdKinematics::Response &res); + bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res); + bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res); + bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res); + bool Pause(kortex_driver::Pause::Request &req, kortex_driver::Pause::Response &res); + bool Resume(kortex_driver::Resume::Request &req, kortex_driver::Resume::Response &res); + bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res); + bool GetCommandedCartesianPose(kortex_driver::GetCommandedCartesianPose::Request &req, kortex_driver::GetCommandedCartesianPose::Response &res); + bool GetTargetedCartesianPose(kortex_driver::GetTargetedCartesianPose::Request &req, kortex_driver::GetTargetedCartesianPose::Response &res); + bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res); + bool GetMeasuredTwist(kortex_driver::GetMeasuredTwist::Request &req, kortex_driver::GetMeasuredTwist::Response &res); + bool GetCommandedTwist(kortex_driver::GetCommandedTwist::Request &req, kortex_driver::GetCommandedTwist::Response &res); + bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res); + bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res); + bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res); + bool GetCommandedJointAngles(kortex_driver::GetCommandedJointAngles::Request &req, kortex_driver::GetCommandedJointAngles::Response &res); + bool SendJointSpeedsCommmand(kortex_driver::SendJointSpeedsCommmand::Request &req, kortex_driver::SendJointSpeedsCommmand::Response &res); + bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res); + bool GetMeasuredJointSpeeds(kortex_driver::GetMeasuredJointSpeeds::Request &req, kortex_driver::GetMeasuredJointSpeeds::Response &res); + bool GetCommandedJointSpeeds(kortex_driver::GetCommandedJointSpeeds::Request &req, kortex_driver::GetCommandedJointSpeeds::Response &res); + bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res); + bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res); + bool GetCommandedGripperMovement(kortex_driver::GetCommandedGripperMovement::Request &req, kortex_driver::GetCommandedGripperMovement::Response &res); + bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res); + bool SetTwistWrenchReferenceFrame(kortex_driver::SetTwistWrenchReferenceFrame::Request &req, kortex_driver::SetTwistWrenchReferenceFrame::Response &res); + bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res); + bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res); + bool ClearFaults(kortex_driver::ClearFaults::Request &req, kortex_driver::ClearFaults::Response &res); + bool GetActiveMap(kortex_driver::GetActiveMap::Request &req, kortex_driver::GetActiveMap::Response &res); + bool GetControlMode(kortex_driver::GetControlMode::Request &req, kortex_driver::GetControlMode::Response &res); + bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res); + bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res); + bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res); + bool OnNotificationServoingModeTopic(kortex_driver::ServoingModeTopic::Request &req, kortex_driver::ServoingModeTopic::Response &res); + void cb_ServoingModeTopic(ServoingModeNotification notif); + bool GetSequenceState(kortex_driver::GetSequenceState::Request &req, kortex_driver::GetSequenceState::Response &res); + bool GetProtectionZoneState(kortex_driver::GetProtectionZoneState::Request &req, kortex_driver::GetProtectionZoneState::Response &res); + bool GetActionExecutionState(kortex_driver::GetActionExecutionState::Request &req, kortex_driver::GetActionExecutionState::Response &res); + bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res); + bool RestoreNetworkFactorySettings(kortex_driver::RestoreNetworkFactorySettings::Request &req, kortex_driver::RestoreNetworkFactorySettings::Response &res); + bool Reboot(kortex_driver::Reboot::Request &req, kortex_driver::Reboot::Response &res); + bool OnNotificationFactoryTopic(kortex_driver::FactoryTopic::Request &req, kortex_driver::FactoryTopic::Response &res); + void cb_FactoryTopic(FactoryNotification notif); + bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res); + bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res); + bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res); + bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res); + bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res); + bool OnNotificationNetworkTopic(kortex_driver::NetworkTopic::Request &req, kortex_driver::NetworkTopic::Response &res); + void cb_NetworkTopic(NetworkNotification notif); + bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res); + bool OnNotificationArmStateTopic(kortex_driver::ArmStateTopic::Request &req, kortex_driver::ArmStateTopic::Response &res); + void cb_ArmStateTopic(ArmStateNotification notif); + bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res); + + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + + BaseCyclicClient* m_basecyclic; + BaseClient* m_base; + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_ConfigurationChangeTopic; + ros::Publisher m_pub_MappingInfoTopic; + ros::Publisher m_pub_ControlModeTopic; + ros::Publisher m_pub_OperatingModeTopic; + ros::Publisher m_pub_SequenceInfoTopic; + ros::Publisher m_pub_ProtectionZoneTopic; + ros::Publisher m_pub_UserTopic; + ros::Publisher m_pub_ControllerTopic; + ros::Publisher m_pub_ActionTopic; + ros::Publisher m_pub_RobotEventTopic; + ros::Publisher m_pub_ServoingModeTopic; + ros::Publisher m_pub_FactoryTopic; + ros::Publisher m_pub_NetworkTopic; + ros::Publisher m_pub_ArmStateTopic; +}; +#endif diff --git a/kortex_driver/src/util/diagnostic.h b/kortex_driver/src/util/diagnostic.h new file mode 100644 index 00000000..cfcedd90 --- /dev/null +++ b/kortex_driver/src/util/diagnostic.h @@ -0,0 +1,17 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ + +#include + +struct kortex_error +{ + int error_code; + std::string description; +}; \ No newline at end of file diff --git a/kortex_driver/src/util/math_util.h b/kortex_driver/src/util/math_util.h new file mode 100644 index 00000000..cd4daeb3 --- /dev/null +++ b/kortex_driver/src/util/math_util.h @@ -0,0 +1,13 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ + +#include + +#define TO_RAD(degree) degree * M_PI / 180.0 \ No newline at end of file diff --git a/kortex_driver/srv/ActionTopic.srv b/kortex_driver/srv/ActionTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ActionTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ActivateMap.srv b/kortex_driver/srv/ActivateMap.srv new file mode 100644 index 00000000..e6b9a564 --- /dev/null +++ b/kortex_driver/srv/ActivateMap.srv @@ -0,0 +1,3 @@ +ActivateMapHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/AddWifiConfiguration.srv b/kortex_driver/srv/AddWifiConfiguration.srv new file mode 100644 index 00000000..48a3c358 --- /dev/null +++ b/kortex_driver/srv/AddWifiConfiguration.srv @@ -0,0 +1,3 @@ +WifiConfiguration input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ApplyEmergencyStop.srv b/kortex_driver/srv/ApplyEmergencyStop.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/ApplyEmergencyStop.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ArmStateTopic.srv b/kortex_driver/srv/ArmStateTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ArmStateTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ChangePassword.srv b/kortex_driver/srv/ChangePassword.srv new file mode 100644 index 00000000..f9fceed1 --- /dev/null +++ b/kortex_driver/srv/ChangePassword.srv @@ -0,0 +1,3 @@ +PasswordChange input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ClearFaults.srv b/kortex_driver/srv/ClearFaults.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/ClearFaults.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ConfigurationChangeTopic.srv b/kortex_driver/srv/ConfigurationChangeTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ConfigurationChangeTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ConnectWifi.srv b/kortex_driver/srv/ConnectWifi.srv new file mode 100644 index 00000000..4e1a65aa --- /dev/null +++ b/kortex_driver/srv/ConnectWifi.srv @@ -0,0 +1,3 @@ +Ssid input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ControlModeTopic.srv b/kortex_driver/srv/ControlModeTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ControlModeTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ControllerTopic.srv b/kortex_driver/srv/ControllerTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ControllerTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateAction.srv b/kortex_driver/srv/CreateAction.srv new file mode 100644 index 00000000..25d11dab --- /dev/null +++ b/kortex_driver/srv/CreateAction.srv @@ -0,0 +1,3 @@ +Action input +--- +ActionHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateMap.srv b/kortex_driver/srv/CreateMap.srv new file mode 100644 index 00000000..93c9e1d4 --- /dev/null +++ b/kortex_driver/srv/CreateMap.srv @@ -0,0 +1,3 @@ +Map input +--- +MapHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateMapping.srv b/kortex_driver/srv/CreateMapping.srv new file mode 100644 index 00000000..241110bf --- /dev/null +++ b/kortex_driver/srv/CreateMapping.srv @@ -0,0 +1,3 @@ +Mapping input +--- +MappingHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateProtectionZone.srv b/kortex_driver/srv/CreateProtectionZone.srv new file mode 100644 index 00000000..12c649ab --- /dev/null +++ b/kortex_driver/srv/CreateProtectionZone.srv @@ -0,0 +1,3 @@ +ProtectionZone input +--- +ProtectionZoneHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateSequence.srv b/kortex_driver/srv/CreateSequence.srv new file mode 100644 index 00000000..884f87c6 --- /dev/null +++ b/kortex_driver/srv/CreateSequence.srv @@ -0,0 +1,3 @@ +Sequence input +--- +SequenceHandle output \ No newline at end of file diff --git a/kortex_driver/srv/CreateUserProfile.srv b/kortex_driver/srv/CreateUserProfile.srv new file mode 100644 index 00000000..9b5dbd6b --- /dev/null +++ b/kortex_driver/srv/CreateUserProfile.srv @@ -0,0 +1,3 @@ +FullUserProfile input +--- +UserProfileHandle output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteAction.srv b/kortex_driver/srv/DeleteAction.srv new file mode 100644 index 00000000..6668d04d --- /dev/null +++ b/kortex_driver/srv/DeleteAction.srv @@ -0,0 +1,3 @@ +ActionHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteAllSequenceTasks.srv b/kortex_driver/srv/DeleteAllSequenceTasks.srv new file mode 100644 index 00000000..00e870c4 --- /dev/null +++ b/kortex_driver/srv/DeleteAllSequenceTasks.srv @@ -0,0 +1,3 @@ +SequenceHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteProtectionZone.srv b/kortex_driver/srv/DeleteProtectionZone.srv new file mode 100644 index 00000000..bc933b51 --- /dev/null +++ b/kortex_driver/srv/DeleteProtectionZone.srv @@ -0,0 +1,3 @@ +ProtectionZoneHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteSequence.srv b/kortex_driver/srv/DeleteSequence.srv new file mode 100644 index 00000000..00e870c4 --- /dev/null +++ b/kortex_driver/srv/DeleteSequence.srv @@ -0,0 +1,3 @@ +SequenceHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteSequenceTask.srv b/kortex_driver/srv/DeleteSequenceTask.srv new file mode 100644 index 00000000..b4170eb4 --- /dev/null +++ b/kortex_driver/srv/DeleteSequenceTask.srv @@ -0,0 +1,3 @@ +SequenceTaskHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteUserProfile.srv b/kortex_driver/srv/DeleteUserProfile.srv new file mode 100644 index 00000000..e5c214e4 --- /dev/null +++ b/kortex_driver/srv/DeleteUserProfile.srv @@ -0,0 +1,3 @@ +UserProfileHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DeleteWifiConfiguration.srv b/kortex_driver/srv/DeleteWifiConfiguration.srv new file mode 100644 index 00000000..4e1a65aa --- /dev/null +++ b/kortex_driver/srv/DeleteWifiConfiguration.srv @@ -0,0 +1,3 @@ +Ssid input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/DisconnectWifi.srv b/kortex_driver/srv/DisconnectWifi.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/DisconnectWifi.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ExecuteAction.srv b/kortex_driver/srv/ExecuteAction.srv new file mode 100644 index 00000000..c74f1249 --- /dev/null +++ b/kortex_driver/srv/ExecuteAction.srv @@ -0,0 +1,3 @@ +Action input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ExecuteActionFromReference.srv b/kortex_driver/srv/ExecuteActionFromReference.srv new file mode 100644 index 00000000..6668d04d --- /dev/null +++ b/kortex_driver/srv/ExecuteActionFromReference.srv @@ -0,0 +1,3 @@ +ActionHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/FactoryTopic.srv b/kortex_driver/srv/FactoryTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/FactoryTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/GetActionExecutionState.srv b/kortex_driver/srv/GetActionExecutionState.srv new file mode 100644 index 00000000..32c9d22d --- /dev/null +++ b/kortex_driver/srv/GetActionExecutionState.srv @@ -0,0 +1,3 @@ +Empty input +--- +ActionExecutionState output \ No newline at end of file diff --git a/kortex_driver/srv/GetActiveMap.srv b/kortex_driver/srv/GetActiveMap.srv new file mode 100644 index 00000000..ea8df28a --- /dev/null +++ b/kortex_driver/srv/GetActiveMap.srv @@ -0,0 +1,3 @@ +MappingHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/GetActuatorCount.srv b/kortex_driver/srv/GetActuatorCount.srv new file mode 100644 index 00000000..cb92302a --- /dev/null +++ b/kortex_driver/srv/GetActuatorCount.srv @@ -0,0 +1,3 @@ +Empty input +--- +ActuatorInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetAllConfiguredWifis.srv b/kortex_driver/srv/GetAllConfiguredWifis.srv new file mode 100644 index 00000000..7a64466e --- /dev/null +++ b/kortex_driver/srv/GetAllConfiguredWifis.srv @@ -0,0 +1,3 @@ +Empty input +--- +WifiConfigurationList output \ No newline at end of file diff --git a/kortex_driver/srv/GetAllConnectedControllers.srv b/kortex_driver/srv/GetAllConnectedControllers.srv new file mode 100644 index 00000000..2397aaff --- /dev/null +++ b/kortex_driver/srv/GetAllConnectedControllers.srv @@ -0,0 +1,3 @@ +Empty input +--- +ControllerList output \ No newline at end of file diff --git a/kortex_driver/srv/GetArmState.srv b/kortex_driver/srv/GetArmState.srv new file mode 100644 index 00000000..9c9bcd89 --- /dev/null +++ b/kortex_driver/srv/GetArmState.srv @@ -0,0 +1,3 @@ +Empty input +--- +ArmStateInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetAvailableWifi.srv b/kortex_driver/srv/GetAvailableWifi.srv new file mode 100644 index 00000000..be8692f3 --- /dev/null +++ b/kortex_driver/srv/GetAvailableWifi.srv @@ -0,0 +1,3 @@ +Empty input +--- +WifiInformationList output \ No newline at end of file diff --git a/kortex_driver/srv/GetCommandedCartesianPose.srv b/kortex_driver/srv/GetCommandedCartesianPose.srv new file mode 100644 index 00000000..a79dc7e6 --- /dev/null +++ b/kortex_driver/srv/GetCommandedCartesianPose.srv @@ -0,0 +1,3 @@ +Empty input +--- +Pose output \ No newline at end of file diff --git a/kortex_driver/srv/GetCommandedGripperMovement.srv b/kortex_driver/srv/GetCommandedGripperMovement.srv new file mode 100644 index 00000000..1311aa77 --- /dev/null +++ b/kortex_driver/srv/GetCommandedGripperMovement.srv @@ -0,0 +1,3 @@ +GripperRequest input +--- +Gripper output \ No newline at end of file diff --git a/kortex_driver/srv/GetCommandedJointAngles.srv b/kortex_driver/srv/GetCommandedJointAngles.srv new file mode 100644 index 00000000..bc24fba9 --- /dev/null +++ b/kortex_driver/srv/GetCommandedJointAngles.srv @@ -0,0 +1,3 @@ +Empty input +--- +JointAngles output \ No newline at end of file diff --git a/kortex_driver/srv/GetCommandedJointSpeeds.srv b/kortex_driver/srv/GetCommandedJointSpeeds.srv new file mode 100644 index 00000000..43ff467a --- /dev/null +++ b/kortex_driver/srv/GetCommandedJointSpeeds.srv @@ -0,0 +1,3 @@ +Empty input +--- +JointSpeeds output \ No newline at end of file diff --git a/kortex_driver/srv/GetCommandedTwist.srv b/kortex_driver/srv/GetCommandedTwist.srv new file mode 100644 index 00000000..fe4d93d5 --- /dev/null +++ b/kortex_driver/srv/GetCommandedTwist.srv @@ -0,0 +1,3 @@ +Empty input +--- +Twist output \ No newline at end of file diff --git a/kortex_driver/srv/GetConfiguredWifi.srv b/kortex_driver/srv/GetConfiguredWifi.srv new file mode 100644 index 00000000..76f8d177 --- /dev/null +++ b/kortex_driver/srv/GetConfiguredWifi.srv @@ -0,0 +1,3 @@ +Ssid input +--- +WifiConfiguration output \ No newline at end of file diff --git a/kortex_driver/srv/GetConnectedWifiInformation.srv b/kortex_driver/srv/GetConnectedWifiInformation.srv new file mode 100644 index 00000000..deced6ec --- /dev/null +++ b/kortex_driver/srv/GetConnectedWifiInformation.srv @@ -0,0 +1,3 @@ +Empty input +--- +WifiInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetControlMode.srv b/kortex_driver/srv/GetControlMode.srv new file mode 100644 index 00000000..6eb15fb1 --- /dev/null +++ b/kortex_driver/srv/GetControlMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +ControlModeInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetControllerState.srv b/kortex_driver/srv/GetControllerState.srv new file mode 100644 index 00000000..5596f9e4 --- /dev/null +++ b/kortex_driver/srv/GetControllerState.srv @@ -0,0 +1,3 @@ +ControllerHandle input +--- +ControllerState output \ No newline at end of file diff --git a/kortex_driver/srv/GetFwdKinematics.srv b/kortex_driver/srv/GetFwdKinematics.srv new file mode 100644 index 00000000..0489367a --- /dev/null +++ b/kortex_driver/srv/GetFwdKinematics.srv @@ -0,0 +1,3 @@ +Empty input +--- +TransformationMatrix output \ No newline at end of file diff --git a/kortex_driver/srv/GetIPv4Configuration.srv b/kortex_driver/srv/GetIPv4Configuration.srv new file mode 100644 index 00000000..1515ce25 --- /dev/null +++ b/kortex_driver/srv/GetIPv4Configuration.srv @@ -0,0 +1,3 @@ +NetworkHandle input +--- +IPv4Configuration output \ No newline at end of file diff --git a/kortex_driver/srv/GetIPv4Information.srv b/kortex_driver/srv/GetIPv4Information.srv new file mode 100644 index 00000000..5b5a39c2 --- /dev/null +++ b/kortex_driver/srv/GetIPv4Information.srv @@ -0,0 +1,3 @@ +NetworkHandle input +--- +IPv4Information output \ No newline at end of file diff --git a/kortex_driver/srv/GetMeasuredCartesianPose.srv b/kortex_driver/srv/GetMeasuredCartesianPose.srv new file mode 100644 index 00000000..a79dc7e6 --- /dev/null +++ b/kortex_driver/srv/GetMeasuredCartesianPose.srv @@ -0,0 +1,3 @@ +Empty input +--- +Pose output \ No newline at end of file diff --git a/kortex_driver/srv/GetMeasuredGripperMovement.srv b/kortex_driver/srv/GetMeasuredGripperMovement.srv new file mode 100644 index 00000000..1311aa77 --- /dev/null +++ b/kortex_driver/srv/GetMeasuredGripperMovement.srv @@ -0,0 +1,3 @@ +GripperRequest input +--- +Gripper output \ No newline at end of file diff --git a/kortex_driver/srv/GetMeasuredJointAngles.srv b/kortex_driver/srv/GetMeasuredJointAngles.srv new file mode 100644 index 00000000..bc24fba9 --- /dev/null +++ b/kortex_driver/srv/GetMeasuredJointAngles.srv @@ -0,0 +1,3 @@ +Empty input +--- +JointAngles output \ No newline at end of file diff --git a/kortex_driver/srv/GetMeasuredJointSpeeds.srv b/kortex_driver/srv/GetMeasuredJointSpeeds.srv new file mode 100644 index 00000000..43ff467a --- /dev/null +++ b/kortex_driver/srv/GetMeasuredJointSpeeds.srv @@ -0,0 +1,3 @@ +Empty input +--- +JointSpeeds output \ No newline at end of file diff --git a/kortex_driver/srv/GetMeasuredTwist.srv b/kortex_driver/srv/GetMeasuredTwist.srv new file mode 100644 index 00000000..fe4d93d5 --- /dev/null +++ b/kortex_driver/srv/GetMeasuredTwist.srv @@ -0,0 +1,3 @@ +Empty input +--- +Twist output \ No newline at end of file diff --git a/kortex_driver/srv/GetOperatingMode.srv b/kortex_driver/srv/GetOperatingMode.srv new file mode 100644 index 00000000..d8364528 --- /dev/null +++ b/kortex_driver/srv/GetOperatingMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +OperatingModeInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetProtectionZoneState.srv b/kortex_driver/srv/GetProtectionZoneState.srv new file mode 100644 index 00000000..9d4d6a1c --- /dev/null +++ b/kortex_driver/srv/GetProtectionZoneState.srv @@ -0,0 +1,3 @@ +ProtectionZoneHandle input +--- +ProtectionZoneInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetSequenceState.srv b/kortex_driver/srv/GetSequenceState.srv new file mode 100644 index 00000000..65790b89 --- /dev/null +++ b/kortex_driver/srv/GetSequenceState.srv @@ -0,0 +1,3 @@ +SequenceHandle input +--- +SequenceInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetServoingMode.srv b/kortex_driver/srv/GetServoingMode.srv new file mode 100644 index 00000000..345f2567 --- /dev/null +++ b/kortex_driver/srv/GetServoingMode.srv @@ -0,0 +1,3 @@ +Empty input +--- +ServoingModeInformation output \ No newline at end of file diff --git a/kortex_driver/srv/GetTargetedCartesianPose.srv b/kortex_driver/srv/GetTargetedCartesianPose.srv new file mode 100644 index 00000000..a79dc7e6 --- /dev/null +++ b/kortex_driver/srv/GetTargetedCartesianPose.srv @@ -0,0 +1,3 @@ +Empty input +--- +Pose output \ No newline at end of file diff --git a/kortex_driver/srv/GetWifiInformation.srv b/kortex_driver/srv/GetWifiInformation.srv new file mode 100644 index 00000000..9208c104 --- /dev/null +++ b/kortex_driver/srv/GetWifiInformation.srv @@ -0,0 +1,3 @@ +Ssid input +--- +WifiInformation output \ No newline at end of file diff --git a/kortex_driver/srv/IsCommunicationInterfaceEnable.srv b/kortex_driver/srv/IsCommunicationInterfaceEnable.srv new file mode 100644 index 00000000..a3ad49c9 --- /dev/null +++ b/kortex_driver/srv/IsCommunicationInterfaceEnable.srv @@ -0,0 +1,3 @@ +NetworkHandle input +--- +CommunicationInterfaceConfiguration output \ No newline at end of file diff --git a/kortex_driver/srv/MappingInfoTopic.srv b/kortex_driver/srv/MappingInfoTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/MappingInfoTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/NetworkTopic.srv b/kortex_driver/srv/NetworkTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/NetworkTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/OperatingModeTopic.srv b/kortex_driver/srv/OperatingModeTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/OperatingModeTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/Pause.srv b/kortex_driver/srv/Pause.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/Pause.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PauseAction.srv b/kortex_driver/srv/PauseAction.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/PauseAction.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PauseSequence.srv b/kortex_driver/srv/PauseSequence.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/PauseSequence.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlayAdvancedSequence.srv b/kortex_driver/srv/PlayAdvancedSequence.srv new file mode 100644 index 00000000..151aa316 --- /dev/null +++ b/kortex_driver/srv/PlayAdvancedSequence.srv @@ -0,0 +1,3 @@ +AdvancedSequenceHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlayCartesianTrajectory.srv b/kortex_driver/srv/PlayCartesianTrajectory.srv new file mode 100644 index 00000000..1a64dd46 --- /dev/null +++ b/kortex_driver/srv/PlayCartesianTrajectory.srv @@ -0,0 +1,3 @@ +ConstrainedPose input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlayCartesianTrajectoryOrientation.srv b/kortex_driver/srv/PlayCartesianTrajectoryOrientation.srv new file mode 100644 index 00000000..5bac9814 --- /dev/null +++ b/kortex_driver/srv/PlayCartesianTrajectoryOrientation.srv @@ -0,0 +1,3 @@ +ConstrainedOrientation input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlayCartesianTrajectoryPosition.srv b/kortex_driver/srv/PlayCartesianTrajectoryPosition.srv new file mode 100644 index 00000000..68c1748f --- /dev/null +++ b/kortex_driver/srv/PlayCartesianTrajectoryPosition.srv @@ -0,0 +1,3 @@ +ConstrainedPosition input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlayJointTrajectory.srv b/kortex_driver/srv/PlayJointTrajectory.srv new file mode 100644 index 00000000..2d17f3ca --- /dev/null +++ b/kortex_driver/srv/PlayJointTrajectory.srv @@ -0,0 +1,3 @@ +ConstrainedJointAngles input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlaySelectedJointTrajectory.srv b/kortex_driver/srv/PlaySelectedJointTrajectory.srv new file mode 100644 index 00000000..b6dfbaa0 --- /dev/null +++ b/kortex_driver/srv/PlaySelectedJointTrajectory.srv @@ -0,0 +1,3 @@ +ConstrainedJointAngle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/PlaySequence.srv b/kortex_driver/srv/PlaySequence.srv new file mode 100644 index 00000000..00e870c4 --- /dev/null +++ b/kortex_driver/srv/PlaySequence.srv @@ -0,0 +1,3 @@ +SequenceHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ProtectionZoneTopic.srv b/kortex_driver/srv/ProtectionZoneTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ProtectionZoneTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAction.srv b/kortex_driver/srv/ReadAction.srv new file mode 100644 index 00000000..f9d93fb0 --- /dev/null +++ b/kortex_driver/srv/ReadAction.srv @@ -0,0 +1,3 @@ +ActionHandle input +--- +Action output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllActions.srv b/kortex_driver/srv/ReadAllActions.srv new file mode 100644 index 00000000..3f1d6d44 --- /dev/null +++ b/kortex_driver/srv/ReadAllActions.srv @@ -0,0 +1,3 @@ +RequestedActionType input +--- +ActionList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllMappings.srv b/kortex_driver/srv/ReadAllMappings.srv new file mode 100644 index 00000000..06aba6a1 --- /dev/null +++ b/kortex_driver/srv/ReadAllMappings.srv @@ -0,0 +1,3 @@ +Empty input +--- +MappingList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllMaps.srv b/kortex_driver/srv/ReadAllMaps.srv new file mode 100644 index 00000000..30e520c4 --- /dev/null +++ b/kortex_driver/srv/ReadAllMaps.srv @@ -0,0 +1,3 @@ +MappingHandle input +--- +MapList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllProtectionZones.srv b/kortex_driver/srv/ReadAllProtectionZones.srv new file mode 100644 index 00000000..31307fba --- /dev/null +++ b/kortex_driver/srv/ReadAllProtectionZones.srv @@ -0,0 +1,3 @@ +Empty input +--- +ProtectionZoneList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllSequences.srv b/kortex_driver/srv/ReadAllSequences.srv new file mode 100644 index 00000000..146a255a --- /dev/null +++ b/kortex_driver/srv/ReadAllSequences.srv @@ -0,0 +1,3 @@ +Empty input +--- +SequenceList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllUserProfiles.srv b/kortex_driver/srv/ReadAllUserProfiles.srv new file mode 100644 index 00000000..46d81410 --- /dev/null +++ b/kortex_driver/srv/ReadAllUserProfiles.srv @@ -0,0 +1,3 @@ +Empty input +--- +UserProfileList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadAllUsers.srv b/kortex_driver/srv/ReadAllUsers.srv new file mode 100644 index 00000000..7ffb2d01 --- /dev/null +++ b/kortex_driver/srv/ReadAllUsers.srv @@ -0,0 +1,3 @@ +Empty input +--- +UserList output \ No newline at end of file diff --git a/kortex_driver/srv/ReadMapping.srv b/kortex_driver/srv/ReadMapping.srv new file mode 100644 index 00000000..27349a2f --- /dev/null +++ b/kortex_driver/srv/ReadMapping.srv @@ -0,0 +1,3 @@ +MappingHandle input +--- +Mapping output \ No newline at end of file diff --git a/kortex_driver/srv/ReadProtectionZone.srv b/kortex_driver/srv/ReadProtectionZone.srv new file mode 100644 index 00000000..bc7600b3 --- /dev/null +++ b/kortex_driver/srv/ReadProtectionZone.srv @@ -0,0 +1,3 @@ +ProtectionZoneHandle input +--- +ProtectionZone output \ No newline at end of file diff --git a/kortex_driver/srv/ReadSequence.srv b/kortex_driver/srv/ReadSequence.srv new file mode 100644 index 00000000..f7fb91c3 --- /dev/null +++ b/kortex_driver/srv/ReadSequence.srv @@ -0,0 +1,3 @@ +SequenceHandle input +--- +Sequence output \ No newline at end of file diff --git a/kortex_driver/srv/ReadUserProfile.srv b/kortex_driver/srv/ReadUserProfile.srv new file mode 100644 index 00000000..58fedf8c --- /dev/null +++ b/kortex_driver/srv/ReadUserProfile.srv @@ -0,0 +1,3 @@ +UserProfileHandle input +--- +UserProfile output \ No newline at end of file diff --git a/kortex_driver/srv/Reboot.srv b/kortex_driver/srv/Reboot.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/Reboot.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/Refresh.srv b/kortex_driver/srv/Refresh.srv new file mode 100644 index 00000000..432de516 --- /dev/null +++ b/kortex_driver/srv/Refresh.srv @@ -0,0 +1,3 @@ +Command input +--- +Feedback output \ No newline at end of file diff --git a/kortex_driver/srv/RefreshCommand.srv b/kortex_driver/srv/RefreshCommand.srv new file mode 100644 index 00000000..9bd2e9fc --- /dev/null +++ b/kortex_driver/srv/RefreshCommand.srv @@ -0,0 +1,3 @@ +Command input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/RefreshCustomData.srv b/kortex_driver/srv/RefreshCustomData.srv new file mode 100644 index 00000000..301250e4 --- /dev/null +++ b/kortex_driver/srv/RefreshCustomData.srv @@ -0,0 +1,3 @@ +CustomData input +--- +CustomData output \ No newline at end of file diff --git a/kortex_driver/srv/RefreshFeedback.srv b/kortex_driver/srv/RefreshFeedback.srv new file mode 100644 index 00000000..c2102f1a --- /dev/null +++ b/kortex_driver/srv/RefreshFeedback.srv @@ -0,0 +1,3 @@ +Empty input +--- +Feedback output \ No newline at end of file diff --git a/kortex_driver/srv/RestoreFactorySettings.srv b/kortex_driver/srv/RestoreFactorySettings.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/RestoreFactorySettings.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/RestoreNetworkFactorySettings.srv b/kortex_driver/srv/RestoreNetworkFactorySettings.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/RestoreNetworkFactorySettings.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/Resume.srv b/kortex_driver/srv/Resume.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/Resume.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ResumeAction.srv b/kortex_driver/srv/ResumeAction.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/ResumeAction.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/ResumeSequence.srv b/kortex_driver/srv/ResumeSequence.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/ResumeSequence.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/RobotEventTopic.srv b/kortex_driver/srv/RobotEventTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/RobotEventTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/SendGripperCommand.srv b/kortex_driver/srv/SendGripperCommand.srv new file mode 100644 index 00000000..f83e0fcd --- /dev/null +++ b/kortex_driver/srv/SendGripperCommand.srv @@ -0,0 +1,3 @@ +GripperCommand input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SendJointSpeedsCommmand.srv b/kortex_driver/srv/SendJointSpeedsCommmand.srv new file mode 100644 index 00000000..18656d38 --- /dev/null +++ b/kortex_driver/srv/SendJointSpeedsCommmand.srv @@ -0,0 +1,3 @@ +JointSpeeds input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SendSelectedJointSpeedCommand.srv b/kortex_driver/srv/SendSelectedJointSpeedCommand.srv new file mode 100644 index 00000000..2cc32856 --- /dev/null +++ b/kortex_driver/srv/SendSelectedJointSpeedCommand.srv @@ -0,0 +1,3 @@ +JointSpeed input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SendTwistCommand.srv b/kortex_driver/srv/SendTwistCommand.srv new file mode 100644 index 00000000..afa8aa1d --- /dev/null +++ b/kortex_driver/srv/SendTwistCommand.srv @@ -0,0 +1,3 @@ +TwistCommand input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SequenceInfoTopic.srv b/kortex_driver/srv/SequenceInfoTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/SequenceInfoTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/ServoingModeTopic.srv b/kortex_driver/srv/ServoingModeTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/ServoingModeTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/SetAdmittance.srv b/kortex_driver/srv/SetAdmittance.srv new file mode 100644 index 00000000..9e132b83 --- /dev/null +++ b/kortex_driver/srv/SetAdmittance.srv @@ -0,0 +1,3 @@ +Admittance input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SetCommunicationInterfaceEnable.srv b/kortex_driver/srv/SetCommunicationInterfaceEnable.srv new file mode 100644 index 00000000..91007ce4 --- /dev/null +++ b/kortex_driver/srv/SetCommunicationInterfaceEnable.srv @@ -0,0 +1,3 @@ +CommunicationInterfaceConfiguration input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SetIPv4Configuration.srv b/kortex_driver/srv/SetIPv4Configuration.srv new file mode 100644 index 00000000..58ed27e3 --- /dev/null +++ b/kortex_driver/srv/SetIPv4Configuration.srv @@ -0,0 +1,3 @@ +FullIPv4Configuration input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SetOperatingMode.srv b/kortex_driver/srv/SetOperatingMode.srv new file mode 100644 index 00000000..a91aa070 --- /dev/null +++ b/kortex_driver/srv/SetOperatingMode.srv @@ -0,0 +1,3 @@ +OperatingModeInformation input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SetServoingMode.srv b/kortex_driver/srv/SetServoingMode.srv new file mode 100644 index 00000000..b8dc46dd --- /dev/null +++ b/kortex_driver/srv/SetServoingMode.srv @@ -0,0 +1,3 @@ +ServoingModeInformation input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/SetTwistWrenchReferenceFrame.srv b/kortex_driver/srv/SetTwistWrenchReferenceFrame.srv new file mode 100644 index 00000000..0ac82544 --- /dev/null +++ b/kortex_driver/srv/SetTwistWrenchReferenceFrame.srv @@ -0,0 +1,3 @@ +CartesianReferenceFrameRequest input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/StartWifiScan.srv b/kortex_driver/srv/StartWifiScan.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/StartWifiScan.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/StopAction.srv b/kortex_driver/srv/StopAction.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/StopAction.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/StopSequence.srv b/kortex_driver/srv/StopSequence.srv new file mode 100644 index 00000000..4d7a11a3 --- /dev/null +++ b/kortex_driver/srv/StopSequence.srv @@ -0,0 +1,3 @@ +Empty input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/Unsubscribe.srv b/kortex_driver/srv/Unsubscribe.srv new file mode 100644 index 00000000..403ee3b9 --- /dev/null +++ b/kortex_driver/srv/Unsubscribe.srv @@ -0,0 +1,3 @@ +NotificationHandle input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/UpdateAction.srv b/kortex_driver/srv/UpdateAction.srv new file mode 100644 index 00000000..c74f1249 --- /dev/null +++ b/kortex_driver/srv/UpdateAction.srv @@ -0,0 +1,3 @@ +Action input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/UpdateProtectionZone.srv b/kortex_driver/srv/UpdateProtectionZone.srv new file mode 100644 index 00000000..1b07d487 --- /dev/null +++ b/kortex_driver/srv/UpdateProtectionZone.srv @@ -0,0 +1,3 @@ +ProtectionZone input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/UpdateSequence.srv b/kortex_driver/srv/UpdateSequence.srv new file mode 100644 index 00000000..5805a3a5 --- /dev/null +++ b/kortex_driver/srv/UpdateSequence.srv @@ -0,0 +1,3 @@ +Sequence input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/UpdateUserProfile.srv b/kortex_driver/srv/UpdateUserProfile.srv new file mode 100644 index 00000000..bbd7d544 --- /dev/null +++ b/kortex_driver/srv/UpdateUserProfile.srv @@ -0,0 +1,3 @@ +UserProfile input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/UserTopic.srv b/kortex_driver/srv/UserTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/UserTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/non_generated/SetApiOptions.srv b/kortex_driver/srv/non_generated/SetApiOptions.srv new file mode 100644 index 00000000..cab7c810 --- /dev/null +++ b/kortex_driver/srv/non_generated/SetApiOptions.srv @@ -0,0 +1,3 @@ +ApiOptions input +--- + diff --git a/kortex_driver/srv/non_generated/SetDeviceID.srv b/kortex_driver/srv/non_generated/SetDeviceID.srv new file mode 100644 index 00000000..396957c5 --- /dev/null +++ b/kortex_driver/srv/non_generated/SetDeviceID.srv @@ -0,0 +1,2 @@ +uint32 device_id +--- diff --git a/kortex_driver/templates/NodeServices.cpp.jinja2 b/kortex_driver/templates/NodeServices.cpp.jinja2 new file mode 100644 index 00000000..d34791c8 --- /dev/null +++ b/kortex_driver/templates/NodeServices.cpp.jinja2 @@ -0,0 +1,145 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +{% for package in detailedPackages %} +{%- if package.HasMessage == 1 -%} +#include "{{package.filename|lower}}_ros_converter.h" +#include "{{package.filename|lower}}_proto_converter.h" +{%- endif %} +{% endfor -%} + +BaseServices::BaseServices(char* ip, ros::NodeHandle& n) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = 0; + m_apiOptions.timeout_ms = 3000; +{% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + m_{{package.name|lower}} = new {{package.name}}::{{package.name}}Client(m_router); + {%- endif -%} + +{% endfor -%} + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + std::cout << "\nSession Created\n"; + + m_pub_Error = m_n.advertise("KortexError", 1000); +{%- for package in detailedPackages -%} +{%- for method in package.service.method -%} +{%- if 'Topic' in method.name %} + m_pub_{{method.name}} = m_n.advertise("{{method.name}}", 1000); +{%- endif -%} +{%- endfor -%} +{%- endfor -%} + + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool BaseServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) +{ + m_CurrentDeviceID = req.device_id; + + return true; +} + +bool BaseServices::SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} + +{% for package in detailedPackages %} +{% for method in package.service.method %} +{%- if 'Topic' in method.name %} +bool BaseServices::OnNotification{{method.name}}(kortex_driver::{{method.name}}::Request &req, kortex_driver::{{method.name}}::Response &res) +{%- else %} +bool BaseServices::{{method.name}}(kortex_driver::{{method.name}}::Request &req, kortex_driver::{{method.name}}::Response &res) +{%- endif %} +{ + {%- set splitInputTypeName = method.input_type.split('.') -%} + {% set splitOutputTypeName = method.output_type.split('.') %} + {{splitInputTypeName[4]}} input; + {%- if not method.input_type.split('.')[4] == "Empty" %} + ToProtoData(req.input, &input); + {%- endif %} + {{splitOutputTypeName[4]}} output; + kortex_driver::KortexError result_error; + + try + { + {%- if not method.output_type.split('.')[4] == "Empty" %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + {%- if 'Topic' in method.name %} + std::function< void ({{package.name}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&BaseServices::cb_{{method.name}}, this, std::placeholders::_1); + output = m_{{package.name|lower}}->OnNotification{{method.name}}(callback, input); + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- else %} + m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- endif %} + } + catch (KDetailedException& ex) + + + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + {%- if not method.output_type.split('.')[4] == "Empty" %} + ToRosData(output, res.output); + {%- endif %} + return true; +} +{%- if 'Topic' in method.name %} +void BaseServices::cb_{{method.name}}({{package.name}}::{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} +{% endfor -%} \ No newline at end of file diff --git a/kortex_driver/templates/NodeServices.h.jinja2 b/kortex_driver/templates/NodeServices.h.jinja2 new file mode 100644 index 00000000..a2f345fc --- /dev/null +++ b/kortex_driver/templates/NodeServices.h.jinja2 @@ -0,0 +1,107 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{packageName}}SERVICES_H_ +#define _KORTEX_{{packageName}}SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +{%- for package in detailedPackages %} +#include <{{package.filename}}.pb.h> +{%- endfor %} + +#include +#include + +#include +#include + +{%- for package in detailedPackages %} +{%- if package.HasRPC == 1 %} +#include <{{package.name}}ClientRpc.h> +{%- endif %} +{%- endfor %} +#include +#include + +{%- for package in detailedPackages %} +{%- for method in package.service.method %} +#include "kortex_driver/{{method.name}}.h" +{%- endfor %} +{%- endfor %} +#include "kortex_driver/KortexError.h" +#include "kortex_driver/SetDeviceID.h" +#include "kortex_driver/SetApiOptions.h" + +#include "kortex_driver/ApiOptions.h" + +using namespace std; +using namespace Kinova::Api; +{%- for package in detailedPackages %} +using namespace {{package.namespace}}; +{%- endfor %} + +class BaseServices +{ + public: + BaseServices(char* ip, ros::NodeHandle& n); + bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res); +{% for package in detailedPackages %} +{%- for method in package.service.method %} +{%- if 'Topic' in method.name %} + bool OnNotification{{method.name}}(kortex_driver::{{method.name}}::Request &req, kortex_driver::{{method.name}}::Response &res); + void cb_{{method.name}}({{method.name|replace("Topic", "")}}Notification notif); +{%- else %} + bool {{method.name}}(kortex_driver::{{method.name}}::Request &req, kortex_driver::{{method.name}}::Response &res); +{%- endif %} +{%- endfor %} +{% endfor %} + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + {% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + {{package.name}}Client* m_{{package.name|lower}}; + {%- endif -%} + {% endfor %} + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + + {%- for package in detailedPackages %} + {%- for method in package.service.method %} + {%- if 'Topic' in method.name %} + ros::Publisher m_pub_{{method.name}}; + {%- endif %} + {%- endfor %} + {%- endfor %} +}; +#endif + diff --git a/kortex_driver/templates/main.jinja2 b/kortex_driver/templates/main.jinja2 new file mode 100644 index 00000000..eba7992f --- /dev/null +++ b/kortex_driver/templates/main.jinja2 @@ -0,0 +1,160 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "math_util.h" + +#include +#include + +#define JOINT_COUNT 7 + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "BaseServices"); + + uint32_t cyclic_data_rate = 100; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 2) + { + ROS_INFO("Connecting to IP = %s - node refresh rate = %s", argv[1], argv[2]); + + //Converting the second parameter(the cyclic rate) to an unsigned int variable. + stringstream tempRate; + tempRate << argv[2]; + tempRate >> cyclic_data_rate; + if(tempRate.fail() || tempRate.bad()) + { + ROS_INFO("ERROR - Bad error rate, shutting down the node..."); + ros::shutdown(); + return 0; + } + } + else + { + ROS_INFO("You need to provide an IP adresse as the first parameter and a cycle rate(Hertz) as the second parameter. ex: rosrun package node 192.168.1.1 100"); + ros::shutdown(); + return 0; + } + + BaseServices services_object(argv[1], n); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &BaseServices::SetDeviceID, &services_object); + + {% for function in list_function -%} + ros::ServiceServer service{{function}} = n.advertiseService("{{function}}", &BaseServices::{{function}}, &services_object); + {% endfor %} + + ROS_INFO("Node's services initialized correctly."); + + ros::Publisher pub_base_feedback = n.advertise("base_feedback", 1000); + ros::Publisher pub_joint_state = n.advertise("base_feedback/joint_state", 1000); + + kortex_driver::Feedback base_feedback; + kortex_driver::RefreshFeedback::Request req; + kortex_driver::RefreshFeedback::Response res; + + sensor_msgs::JointState joint_state; + + ros::Rate rate(cyclic_data_rate); // 100 hz + while (!ros::isShuttingDown()) + { + services_object.RefreshFeedback(req, res); + + base_feedback.frame_id = res.output.frame_id; + + base_feedback.base.arm_voltage = res.output.base.arm_voltage; + base_feedback.base.arm_current = res.output.base.arm_current; + base_feedback.base.temperature_cpu = res.output.base.temperature_cpu; + base_feedback.base.temperature_ambient = res.output.base.temperature_ambient; + base_feedback.base.imu_acceleration_x = res.output.base.imu_acceleration_x; + base_feedback.base.imu_acceleration_y = res.output.base.imu_acceleration_y; + base_feedback.base.imu_acceleration_z = res.output.base.imu_acceleration_z; + base_feedback.base.imu_angular_velocity_x = res.output.base.imu_angular_velocity_x; + base_feedback.base.imu_angular_velocity_y = res.output.base.imu_angular_velocity_y; + base_feedback.base.imu_angular_velocity_z = res.output.base.imu_angular_velocity_z; + base_feedback.base.tool_pose_x = res.output.base.tool_pose_x; + base_feedback.base.tool_pose_y = res.output.base.tool_pose_y; + base_feedback.base.tool_pose_z = res.output.base.tool_pose_z; + base_feedback.base.tool_pose_theta_x = res.output.base.tool_pose_theta_x; + base_feedback.base.tool_pose_theta_y = res.output.base.tool_pose_theta_y; + base_feedback.base.tool_pose_theta_z = res.output.base.tool_pose_theta_z; + base_feedback.base.tool_external_wrench_force_x = res.output.base.tool_external_wrench_force_x; + base_feedback.base.tool_external_wrench_force_y = res.output.base.tool_external_wrench_force_y; + base_feedback.base.tool_external_wrench_force_z = res.output.base.tool_external_wrench_force_z; + base_feedback.base.tool_external_wrench_torque_x = res.output.base.tool_external_wrench_torque_x; + base_feedback.base.tool_external_wrench_torque_y = res.output.base.tool_external_wrench_torque_y; + base_feedback.base.tool_external_wrench_torque_z = res.output.base.tool_external_wrench_torque_z; + base_feedback.base.fault_bank_a = res.output.base.fault_bank_a; + base_feedback.base.fault_bank_b = res.output.base.fault_bank_b; + base_feedback.base.warning_bank_a = res.output.base.warning_bank_a; + base_feedback.base.warning_bank_b = res.output.base.warning_bank_b; + + base_feedback.actuators.clear(); + + joint_state.position.resize(JOINT_COUNT); + joint_state.velocity.resize(JOINT_COUNT); + joint_state.effort.resize(JOINT_COUNT); + joint_state.name.resize(JOINT_COUNT); + + for(int i = 0; i < JOINT_COUNT; i++) + { + kortex_driver::ActuatorFeedback temp; + + temp.status_flags = res.output.actuators[i].status_flags; + temp.jitter_comm = res.output.actuators[i].jitter_comm; + temp.position = res.output.actuators[i].position; + temp.velocity = res.output.actuators[i].velocity; + temp.torque = res.output.actuators[i].torque; + temp.current_motor = res.output.actuators[i].current_motor; + temp.voltage = res.output.actuators[i].voltage; + temp.temperature_motor = res.output.actuators[i].temperature_motor; + temp.temperature_core = res.output.actuators[i].temperature_core; + temp.fault_bank_a = res.output.actuators[i].fault_bank_a; + temp.fault_bank_b = res.output.actuators[i].fault_bank_b; + temp.warning_bank_a = res.output.actuators[i].warning_bank_a; + temp.warning_bank_b = res.output.actuators[i].warning_bank_b; + + base_feedback.actuators.push_back(temp); + + joint_state.name[i] = "Actuator" + std::to_string(i + 1); + joint_state.position[i] = TO_RAD(res.output.actuators[i].position); + joint_state.velocity[i] = TO_RAD(res.output.actuators[i].velocity); + joint_state.effort[i] = res.output.actuators[i].torque; + } + base_feedback.interconnect.position = res.output.interconnect.position; + + + + joint_state.header.stamp = ros::Time::now(); + joint_state.header.frame_id = std::to_string(res.output.frame_id); + + pub_base_feedback.publish(base_feedback); + pub_joint_state.publish(joint_state); + + + ros::spinOnce(); + + + + rate.sleep(); + } + + return 1; +} diff --git a/kortex_driver/templates/proto_converter.cpp.jinja2 b/kortex_driver/templates/proto_converter.cpp.jinja2 new file mode 100644 index 00000000..e37a427f --- /dev/null +++ b/kortex_driver/templates/proto_converter.cpp.jinja2 @@ -0,0 +1,77 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentPackageName|lower}}_proto_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_proto_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToProtoData(kortex_driver::{{detailed_message.message.name}} input, {{detailed_message.message.name}} *output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") -%} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {%- if field.type == 11 %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + ToProtoData(input.{{field.name}}[i], output->add_{{field.name|lower}}()); + } + {%- else %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name|lower}}.size(); i++) + { + output->add_{{field.name|lower}}(input.{{field.name|lower}}[i]); + } + {% endif -%} + {% else -%} + {%- if field.type == 11 %} + ToProtoData(input.{{field.name}}, output->mutable_{{field.name}}()); + {%- elif field.type == 14 %}{# ENUM #} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output->set_{{field.name}}(({{field.type_name|replace(".", "", 1)|replace(".", "::")}})input.{{field.name}}); + {%- elif field.type == 12 %} + output->set_{{field.name}}(std::string(input.{{field.name}}.begin(), input.{{field.name}}.end())); + {%- else %} + output->set_{{field.name}}(input.{{field.name}}); + {%- endif -%} + {%- endif -%} + {%- endif -%} + {%- endfor -%} + + {% if detailed_message.HasOneOf == "true" %} + + {% for field in detailed_message.message.field %} + {%- if field.HasField("oneof_index") -%} + if(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.size() > 0) + { + {% if field.type == 11 -%} + ToProtoData(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0], output->mutable_{{field.name}}()); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} + output->set_{{field.name}}(({{list1[4]}})input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- else %} + output->set_{{field.name}}(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- endif %} + } + {% endif %} + {%- endfor -%} + {% endif %} + + return 0; +} +{% endfor %} diff --git a/kortex_driver/templates/proto_converter.h.jinja2 b/kortex_driver/templates/proto_converter.h.jinja2 new file mode 100644 index 00000000..ec4abc80 --- /dev/null +++ b/kortex_driver/templates/proto_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToProtoData(kortex_driver::{{detailed_message.message.name}} intput, {{detailed_message.message.name}} *output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_driver/templates/ros_converter.cpp.jinja2 b/kortex_driver/templates/ros_converter.cpp.jinja2 new file mode 100644 index 00000000..80fd8dde --- /dev/null +++ b/kortex_driver/templates/ros_converter.cpp.jinja2 @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentPackageName|lower}}_ros_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_ros_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_driver::{{detailed_message.message.name}} &output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") %} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {% if field.type == 11 %} + {%- set splitTypeName = field.type_name.split('.') -%} + output.{{field.name|lower}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + kortex_driver::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(i), temp); + output.{{field.name}}.push_back(temp); + } + {%- else %} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + output.{{field.name}}.push_back(input.{{field.name|lower}}(i)); + } + {%- endif %} + {%- else %} + {%- if field.type == 11 %} + ToRosData(input.{{field.name}}(), output.{{field.name}}); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output.{{field.name}} = input.{{field.name}}(); + {%- elif field.type == 12 %} + output.{{field.name}} = std::vector(input.{{field.name}}().begin(), input.{{field.name}}().end()); + {%- else %} + output.{{field.name}} = input.{{field.name}}(); + {%- endif %} + {%- endif %} + {%- endif %} + {%- endfor %} + + {% if detailed_message.HasOneOf == "true" %} + auto oneof_type = input.{{detailed_message.message.ListFields()[-1][1][0].name}}_case(); + + switch(oneof_type) + { + {%- for field in detailed_message.message.field -%} + {%- if field.HasField("oneof_index") -%} + {%- set splitTypeName = field.type_name.split('.') %} + {%- set EnumName = field.name.replace("_", " ").title().replace(" ", "") %} + case {{detailed_message.message.name}}::k{{EnumName}}: + { + {%- if field.type == 11 %} + kortex_driver::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(), temp); + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(temp); + {%- elif field.type == 14 %} + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(input.{{field.name}}()); + {% endif %} + break; + } + {% endif %} + {%- endfor %} + } + {% endif -%} + + return 0; +} +{% endfor %} diff --git a/kortex_driver/templates/ros_converter.h.jinja2 b/kortex_driver/templates/ros_converter.h.jinja2 new file mode 100644 index 00000000..f7162018 --- /dev/null +++ b/kortex_driver/templates/ros_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_driver::{{detailed_message.message.name}} &output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_driver/templates/ros_enum.jinja2 b/kortex_driver/templates/ros_enum.jinja2 new file mode 100644 index 00000000..164146fc --- /dev/null +++ b/kortex_driver/templates/ros_enum.jinja2 @@ -0,0 +1,3 @@ +{% for member in item.value %} +uint32 {{member.name}} = {{member.number}} +{% endfor %} diff --git a/kortex_driver/templates/ros_message.jinja2 b/kortex_driver/templates/ros_message.jinja2 new file mode 100644 index 00000000..746188b6 --- /dev/null +++ b/kortex_driver/templates/ros_message.jinja2 @@ -0,0 +1,44 @@ +{%- for member in item.field -%} +{%- if not member.HasField("oneof_index") -%} +{%- if member.type == 9 %} {# TYPE_STRING #} +string{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 12 %} {# TYPE_BYTES #} +uint8[] {{member.name}} +{%- elif member.type == 1 %} {# TYPE_DOUBLE #} +float64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 7 %} {# TYPE_FIXED32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 6 %} {# TYPE_FIXED64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 2 %} {# TYPE_FLOAT #} +float32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 5 %} {# TYPE_INT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 3 %} {# TYPE_INT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 15 %} {# TYPE_SFIXED32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 16 %} {# TYPE_SFIXED64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 17 %} {# TYPE_SINT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 18 %} {# TYPE_SINT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 13 %} {# TYPE_UINT32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 4 %} {# TYPE_UINT64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 14 -%} {# TYPE_ENUM #} +{% set list1 = member.type_name.split('.') %} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 8 %} {# TYPE_BOOL #} +bool {{member.name}} +{%- elif member.type == 11 %}{# TYPE MESSAGE #} +{% set list1 = member.type_name.split('.') %} +{{list1[4]}}{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- endif -%} +{%- endif -%} +{%- endfor -%} +{%- if HasOneOf %} +{{item.name}}_{{item.ListFields()[-1][1][0].name}} oneof_{{item.ListFields()[-1][1][0].name}} +{%- endif -%} \ No newline at end of file diff --git a/kortex_driver/templates/ros_oneof.jinja2 b/kortex_driver/templates/ros_oneof.jinja2 new file mode 100644 index 00000000..4fac302a --- /dev/null +++ b/kortex_driver/templates/ros_oneof.jinja2 @@ -0,0 +1,9 @@ +{%- for member in item.field -%} +{% if member.HasField("oneof_index") %} +{% if member.type == 11 %} +{% set list1 = member.type_name.split('.') %}{{list1[4]}}[] {{member.name}} +{%- else -%} +uint32[] {{member.name}} +{%- endif -%} +{%- endif -%} +{% endfor %} \ No newline at end of file diff --git a/kortex_driver/templates/ros_service.jinja2 b/kortex_driver/templates/ros_service.jinja2 new file mode 100644 index 00000000..cc015cf8 --- /dev/null +++ b/kortex_driver/templates/ros_service.jinja2 @@ -0,0 +1,5 @@ +{% set split_input_type = item.input_type.split('.') %} +{%- set split_output_type = item.output_type.split('.') -%} +{{split_input_type[4]}} input +--- +{{split_output_type[4]}} output \ No newline at end of file diff --git a/kortex_examples/CMakeLists.txt b/kortex_examples/CMakeLists.txt new file mode 100644 index 00000000..777c4563 --- /dev/null +++ b/kortex_examples/CMakeLists.txt @@ -0,0 +1,40 @@ + +cmake_minimum_required(VERSION 2.8.3) +project(kortex_examples) + +add_compile_options(-std=c++11) +add_definitions(-D_OS_UNIX) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation kortex_driver kortex_actuator_driver kortex_vision_config_driver kortex_device_manager) + +file(GLOB_RECURSE cpp_list RELATIVE ${PROJECT_SOURCE_DIR} "cpp/*.cpp") + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${PROJECT_SOURCE_DIR}/src) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client_stubs) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/messages) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/common) +include_directories(include ${PROJECT_SOURCE_DIR}/src/util) + +add_executable(PlayCartesian cpp/example_play_cartesian.cpp) +target_link_libraries(PlayCartesian ${catkin_LIBRARIES} ) + +add_executable(PlayCartesianPosition cpp/example_play_cartesian_position.cpp) +target_link_libraries(PlayCartesianPosition ${catkin_LIBRARIES} ) + +add_executable(GetControlLoopParameters cpp/example_get_control_loop_parameters.cpp) +target_link_libraries(GetControlLoopParameters ${catkin_LIBRARIES} ) + +add_executable(SetControlLoopParameters cpp/example_set_control_loop_parameters.cpp) +target_link_libraries(SetControlLoopParameters ${catkin_LIBRARIES} ) + +add_executable(GetSensorSettings cpp/example_get_sensor_settings.cpp) +target_link_libraries(GetSensorSettings ${catkin_LIBRARIES} ) + +add_executable(ReadAllDevices cpp/example_read_all_devices.cpp) +target_link_libraries(ReadAllDevices ${catkin_LIBRARIES} ) diff --git a/kortex_examples/cpp/example_get_control_loop_parameters.cpp b/kortex_examples/cpp/example_get_control_loop_parameters.cpp new file mode 100644 index 00000000..8cc87c04 --- /dev/null +++ b/kortex_examples/cpp/example_get_control_loop_parameters.cpp @@ -0,0 +1,85 @@ +#include "ros/ros.h" +#include +#include +#include "kortex_actuator_driver/SetDeviceID.h" +#include "kortex_device_manager/DeviceTypes.h" +#include "kortex_device_manager/ReadAllDevices.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Actuator_GetControlLoopParameters"); + + ros::NodeHandle n; + + ros::ServiceClient service_GetControlLoopParameters = n.serviceClient("GetControlLoopParameters"); + ros::ServiceClient service_SetDeviceID = n.serviceClient("SetDeviceID"); + ros::ServiceClient service_ReadAllDevices = n.serviceClient("ReadAllDevices"); + + kortex_actuator_driver::GetControlLoopParameters srvGetControlLoopParameters; + kortex_actuator_driver::SetDeviceID srvSetDeviceID; + kortex_device_manager::ReadAllDevices srvReadAllDevices; + + kortex_actuator_driver::ControlLoopSelection loopSelection; + kortex_device_manager::DeviceTypes device_type; + + srvGetControlLoopParameters.request.input.loop_selection = loopSelection.MOTOR_VELOCITY; + + bool actuatorFound = false; + uint32_t actuator_id = 0; + + if (service_ReadAllDevices.call(srvReadAllDevices)) + { + + for(int i = 0; i < srvReadAllDevices.response.output.device_handle.size(); i++) + { + if((srvReadAllDevices.response.output.device_handle[i].device_type == device_type.BIG_ACTUATOR) || + (srvReadAllDevices.response.output.device_handle[i].device_type == device_type.SMALL_ACTUATOR)) + { + + actuator_id = srvReadAllDevices.response.output.device_handle[i].device_identifier; + actuatorFound = true; + srvSetDeviceID.request.device_id = actuator_id; + + if (!service_SetDeviceID.call(srvSetDeviceID)) + { + ROS_ERROR("Failed to call SetDeviceID"); + } + + break; + } + } + } + else + { + ROS_ERROR("Failed to call ReadAllDevices"); + return 1; + } + + if(actuatorFound) + { + ROS_INFO("Found 1 actuator.\n"); + + if (service_GetControlLoopParameters.call(srvGetControlLoopParameters)) + { + ROS_INFO("loop_selection: %d", srvGetControlLoopParameters.response.output.loop_selection); + ROS_INFO("error_saturation: %f", srvGetControlLoopParameters.response.output.error_saturation); + ROS_INFO("output_saturation: %f", srvGetControlLoopParameters.response.output.output_saturation); + + for(int i = 0; i < srvGetControlLoopParameters.response.output.kAz.size(); i++) + { + ROS_INFO("kAz[%d]: %f", i, srvGetControlLoopParameters.response.output.kAz[i]); + } + for(int i = 0; i < srvGetControlLoopParameters.response.output.kBz.size(); i++) + { + ROS_INFO("kBz[%d]: %f", i, srvGetControlLoopParameters.response.output.kBz[i]); + } + } + else + { + ROS_ERROR("Failed to call GetControlLoopParameters"); + return 1; + } + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/cpp/example_get_sensor_settings.cpp b/kortex_examples/cpp/example_get_sensor_settings.cpp new file mode 100644 index 00000000..5b106c30 --- /dev/null +++ b/kortex_examples/cpp/example_get_sensor_settings.cpp @@ -0,0 +1,80 @@ +#include "ros/ros.h" +#include "kortex_vision_config_driver/GetSensorSettings.h" +#include "kortex_vision_config_driver/Sensor.h" +#include "kortex_vision_config_driver/SetDeviceID.h" +#include "kortex_device_manager/DeviceTypes.h" +#include "kortex_device_manager/ReadAllDevices.h" +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "VisionConfig_GetSensorSettings"); + + ros::NodeHandle n; + + ros::ServiceClient service_GetSensorSettings = n.serviceClient("GetSensorSettings"); + ros::ServiceClient service_SetDeviceID = n.serviceClient("SetDeviceID"); + ros::ServiceClient service_ReadAllDevices = n.serviceClient("ReadAllDevices"); + + kortex_vision_config_driver::GetSensorSettings srvGetSensorSettings; + kortex_vision_config_driver::SetDeviceID srvSetDeviceID; + kortex_device_manager::ReadAllDevices srvReadAllDevices; + + kortex_vision_config_driver::Sensor sensor_type; + kortex_device_manager::DeviceTypes device_type; + + srvGetSensorSettings.request.input.sensor = sensor_type.SENSOR_COLOR; + + bool visionFound = false; + uint32_t vision_id = 0; + + if (service_ReadAllDevices.call(srvReadAllDevices)) + { + + for(int i = 0; i < srvReadAllDevices.response.output.device_handle.size(); i++) + { + if(srvReadAllDevices.response.output.device_handle[i].device_type == device_type.VISION) + { + + vision_id = srvReadAllDevices.response.output.device_handle[i].device_identifier; + visionFound = true; + srvSetDeviceID.request.device_id = vision_id; + + if (!service_SetDeviceID.call(srvSetDeviceID)) + { + ROS_ERROR("Failed to call SetDeviceID"); + } + } + } + } + else + { + ROS_ERROR("Failed to call ReadAllDevices"); + return 1; + } + + if(visionFound) + { + ROS_INFO("Found 1 vision module.\n"); + + if (service_GetSensorSettings.call(srvGetSensorSettings)) + { + ROS_INFO("sensor: %d", srvGetSensorSettings.response.output.sensor); + ROS_INFO("resolution: %d", srvGetSensorSettings.response.output.resolution); + ROS_INFO("frame_rate: %d", srvGetSensorSettings.response.output.frame_rate); + ROS_INFO("bit_rate: %d", srvGetSensorSettings.response.output.bit_rate); + } + else + { + ROS_ERROR("Failed to call GetSensorSettings"); + return 1; + } + } + else + { + ROS_INFO("Could not find any vision module on the target robot."); + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/cpp/example_play_cartesian.cpp b/kortex_examples/cpp/example_play_cartesian.cpp new file mode 100644 index 00000000..ff7a5655 --- /dev/null +++ b/kortex_examples/cpp/example_play_cartesian.cpp @@ -0,0 +1,72 @@ +#include "ros/ros.h" +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Sequence"); + + ros::NodeHandle n; + + ros::ServiceClient client_PlayCartesianTrajectory = n.serviceClient("PlayCartesianTrajectory"); + ros::ServiceClient client_RefreshFeedback = n.serviceClient("RefreshFeedback"); + + kortex_driver::PlayCartesianTrajectory srvPlayCartesianTrajectory; + kortex_driver::RefreshFeedback srvRefreshFeedback; + + float current_x = 0.0f; + float current_y = 0.0f; + float current_z = 0.0f; + + float current_theta_x = 0.0f; + float current_theta_y = 0.0f; + float current_theta_z = 0.0f; + + if (client_RefreshFeedback.call(srvRefreshFeedback)) + { + current_x = srvRefreshFeedback.response.output.base.tool_pose_x; + current_y = srvRefreshFeedback.response.output.base.tool_pose_y; + current_z = srvRefreshFeedback.response.output.base.tool_pose_z; + + current_theta_x = srvRefreshFeedback.response.output.base.tool_pose_theta_x; + current_theta_y = srvRefreshFeedback.response.output.base.tool_pose_theta_y; + current_theta_z = srvRefreshFeedback.response.output.base.tool_pose_theta_z; + + ROS_INFO("Getting cyclic data from the robot - x= %f y= %f z= %f theta x = %f theta y = %f theta z = %f", + current_x, current_y, current_z, current_theta_x, current_theta_y, current_theta_z); + } + else + { + ROS_ERROR("Failed to retrieve the cyclic data."); + return 1; + } + + //Creating our next target (a Cartesian pose) + srvPlayCartesianTrajectory.request.input.target_pose.x = current_x; + srvPlayCartesianTrajectory.request.input.target_pose.y = current_y; + srvPlayCartesianTrajectory.request.input.target_pose.z = current_z + 0.1; + + srvPlayCartesianTrajectory.request.input.target_pose.theta_x = current_theta_x; + srvPlayCartesianTrajectory.request.input.target_pose.theta_y = current_theta_y; + srvPlayCartesianTrajectory.request.input.target_pose.theta_z = current_theta_z + 60; + + kortex_driver::CartesianSpeed poseSpeed; + poseSpeed.translation = 0.1; + poseSpeed.orientation = 15; + + srvPlayCartesianTrajectory.request.input.constraint.oneof_type.speed.push_back(poseSpeed); + + ROS_INFO("CALL PlayCartesianTrajectory"); + if (client_PlayCartesianTrajectory.call(srvPlayCartesianTrajectory)) + { + ROS_INFO("pose sent"); + } + else + { + ROS_ERROR("Failed to call PlayCartesianTrajectory"); + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/cpp/example_play_cartesian_position.cpp b/kortex_examples/cpp/example_play_cartesian_position.cpp new file mode 100644 index 00000000..8688acb7 --- /dev/null +++ b/kortex_examples/cpp/example_play_cartesian_position.cpp @@ -0,0 +1,59 @@ +#include "ros/ros.h" +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "example_play_cartesian_position"); + + ros::NodeHandle n; + + ros::ServiceClient client_PlayCartesianTrajectoryPosition = n.serviceClient("PlayCartesianTrajectoryPosition"); + ros::ServiceClient client_RefreshFeedback = n.serviceClient("RefreshFeedback"); + + kortex_driver::PlayCartesianTrajectoryPosition srvPlayCartesianTrajectoryPosition; + kortex_driver::RefreshFeedback srvRefreshFeedback; + + float current_x = 0.0f; + float current_y = 0.0f; + float current_z = 0.0f; + + if (client_RefreshFeedback.call(srvRefreshFeedback)) + { + current_x = srvRefreshFeedback.response.output.base.tool_pose_x; + current_y = srvRefreshFeedback.response.output.base.tool_pose_y; + current_z = srvRefreshFeedback.response.output.base.tool_pose_z; + ROS_INFO("Getting cyclic data from the robot - x= %f y= %f z= %f", current_x, current_y, current_z); + } + else + { + ROS_ERROR("Failed to retrieve the cyclic data."); + return 1; + } + + + + //Creating our next target (a Cartesian pose) + srvPlayCartesianTrajectoryPosition.request.input.target_position.x = current_x; + srvPlayCartesianTrajectoryPosition.request.input.target_position.y = current_y; + srvPlayCartesianTrajectoryPosition.request.input.target_position.z = current_z + 0.1; + + kortex_driver::CartesianSpeed poseSpeed; + poseSpeed.translation = 0.1; + + srvPlayCartesianTrajectoryPosition.request.input.constraint.oneof_type.speed.push_back(poseSpeed); + + ROS_INFO("CALL PlayCartesianTrajectory"); + if (client_PlayCartesianTrajectoryPosition.call(srvPlayCartesianTrajectoryPosition)) + { + ROS_INFO("pose sent"); + } + else + { + ROS_ERROR("Failed to call PlayCartesianTrajectoryPosition"); + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/cpp/example_read_all_devices.cpp b/kortex_examples/cpp/example_read_all_devices.cpp new file mode 100644 index 00000000..1345907b --- /dev/null +++ b/kortex_examples/cpp/example_read_all_devices.cpp @@ -0,0 +1,34 @@ +#include "ros/ros.h" +#include "kortex_device_manager/ReadAllDevices.h" +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Example_DeviceManager_ReadAllDevices"); + + ros::NodeHandle n; + + ros::ServiceClient service_ReadAllDevices = n.serviceClient("ReadAllDevices"); + + kortex_device_manager::ReadAllDevices srvReadAllDevices; + + ROS_INFO("CALL ReadAllDevices"); + + if (service_ReadAllDevices.call(srvReadAllDevices)) + { + for(int i = 0; i < srvReadAllDevices.response.output.device_handle.size(); i++) + { + ROS_INFO("device_type: %d", srvReadAllDevices.response.output.device_handle[i].device_type); + ROS_INFO("device_identifier: %d", srvReadAllDevices.response.output.device_handle[i].device_identifier); + ROS_INFO("order: %d", srvReadAllDevices.response.output.device_handle[i].order); + } + } + else + { + ROS_ERROR("Failed to call ReadAllDevices"); + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/cpp/example_set_control_loop_parameters.cpp b/kortex_examples/cpp/example_set_control_loop_parameters.cpp new file mode 100644 index 00000000..f6cd53d6 --- /dev/null +++ b/kortex_examples/cpp/example_set_control_loop_parameters.cpp @@ -0,0 +1,72 @@ +#include "ros/ros.h" +#include "kortex_actuator_driver/SetControlLoopParameters.h" +#include "kortex_actuator_driver/ControlLoopSelection.h" +#include "kortex_actuator_driver/SetDeviceID.h" +#include "kortex_device_manager/DeviceTypes.h" +#include "kortex_device_manager/ReadAllDevices.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Example_Actuator_SetControlLoopParameters"); + + ros::NodeHandle n; + + ros::ServiceClient service_SetControlLoopParameters = n.serviceClient("SetControlLoopParameters"); + ros::ServiceClient service_SetDeviceID = n.serviceClient("SetDeviceID"); + ros::ServiceClient service_ReadAllDevices = n.serviceClient("ReadAllDevices"); + + + kortex_actuator_driver::SetControlLoopParameters srvSetControlLoopParameters; + kortex_actuator_driver::ControlLoopSelection loopSelection; + kortex_actuator_driver::SetDeviceID srvSetDeviceID; + kortex_device_manager::ReadAllDevices srvReadAllDevices; + + srvSetControlLoopParameters.request.input.loop_selection = loopSelection.MOTOR_VELOCITY; + kortex_device_manager::DeviceTypes device_type; + + bool actuatorFound = false; + uint32_t actuator_id = 0; + + if (service_ReadAllDevices.call(srvReadAllDevices)) + { + + for(int i = 0; i < srvReadAllDevices.response.output.device_handle.size(); i++) + { + if((srvReadAllDevices.response.output.device_handle[i].device_type == device_type.BIG_ACTUATOR) || + (srvReadAllDevices.response.output.device_handle[i].device_type == device_type.SMALL_ACTUATOR)) + { + + actuator_id = srvReadAllDevices.response.output.device_handle[i].device_identifier; + actuatorFound = true; + srvSetDeviceID.request.device_id = actuator_id; + + if (!service_SetDeviceID.call(srvSetDeviceID)) + { + ROS_ERROR("Failed to call SetDeviceID"); + } + + break; + } + } + } + else + { + ROS_ERROR("Failed to call ReadAllDevices"); + return 1; + } + + if(actuatorFound) + { + if (service_SetControlLoopParameters.call(srvSetControlLoopParameters)) + { + ROS_INFO("ControlLoopParameters sent with success"); + } + else + { + ROS_ERROR("Failed to call SetControlLoopParameters"); + return 1; + } + } + + return 0; +} \ No newline at end of file diff --git a/kortex_examples/package.xml b/kortex_examples/package.xml new file mode 100644 index 00000000..38f6ad99 --- /dev/null +++ b/kortex_examples/package.xml @@ -0,0 +1,34 @@ + + + kortex_examples + 1.0.0 + THe kortex package that act as a robot's driver. + + KINOVA + + BSD + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_generation + kortex_driver + kortex_actuator_driver + kortex_vision_config_driver + kortex_device_manager + message_runtime + + + + + + + + diff --git a/kortex_examples/python/GetControlLoopParameters.py b/kortex_examples/python/GetControlLoopParameters.py new file mode 100644 index 00000000..1a2f950b --- /dev/null +++ b/kortex_examples/python/GetControlLoopParameters.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +import sys +import rospy +import time +from kortex_actuator_driver.srv import * +from kortex_actuator_driver.msg import * +from kortex_device_manager.srv import * +from kortex_device_manager.msg import * + +def GetControlLoopParameters_Client(): + rospy.wait_for_service('GetControlLoopParameters') + + try: + function_GetControlLoopParameters = rospy.ServiceProxy('GetControlLoopParameters', GetControlLoopParameters) + function_SetDeviceID = rospy.ServiceProxy('SetDeviceID', SetDeviceID) + function_ReadAllDevices = rospy.ServiceProxy('ReadAllDevices', ReadAllDevices) + + reqGetControlLoopParameters = GetControlLoopParametersRequest() + reqReadAllDevices = ReadAllDevicesRequest() + reqSetDeviceID = SetDeviceIDRequest() + + reqGetControlLoopParameters.input.loop_selection = ControlLoopSelection.MOTOR_VELOCITY; + + actuatorFound = False; + actuator_id = 0; + + responseReadAllDevices = function_ReadAllDevices(reqReadAllDevices) + + for index, device in enumerate(responseReadAllDevices.output.device_handle): + if device.device_type is DeviceTypes.BIG_ACTUATOR or device.device_type is DeviceTypes.SMALL_ACTUATOR: + actuator_id = device.device_identifier + actuatorFound = True + reqSetDeviceID.device_id = actuator_id + function_SetDeviceID(reqSetDeviceID) + break + + if actuatorFound: + responseGetControlLoopParameters = function_GetControlLoopParameters(reqGetControlLoopParameters) + + print("loop_selection = {}".format(responseGetControlLoopParameters.output.loop_selection)) + print("error_saturation = {}".format(responseGetControlLoopParameters.output.error_saturation)) + print("output_saturation = {}\n".format(responseGetControlLoopParameters.output.output_saturation)) + + for index, gainA in enumerate(responseGetControlLoopParameters.output.kAz): + print("kAz[{}] = {}".format(index, gainA)) + + print("\n") + + for index, gainB in enumerate(responseGetControlLoopParameters.output.kBz): + print("kBz[{}] = {}".format(index, gainB)) + + except rospy.ServiceException as e: + print "Service call failed: %s"%e + +if __name__ == "__main__": + GetControlLoopParameters_Client() \ No newline at end of file diff --git a/kortex_examples/python/GetSensorSettings.py b/kortex_examples/python/GetSensorSettings.py new file mode 100644 index 00000000..b1176c54 --- /dev/null +++ b/kortex_examples/python/GetSensorSettings.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python + +import sys +import rospy +import time +from kortex_vision_config_driver.srv import * +from kortex_vision_config_driver.msg import * + +def GetSensorSettings_Client(): + rospy.wait_for_service('GetSensorSettings') + try: + function_GetSensorSettings = rospy.ServiceProxy('GetSensorSettings', GetSensorSettings) + + req = GetSensorSettingsRequest() + + req.input.sensor = Sensor.SENSOR_COLOR; + + response = function_GetSensorSettings(req) + + print(response) + + + + except rospy.ServiceException as e: + print "Service call failed: %s"%e + +if __name__ == "__main__": + GetSensorSettings_Client() \ No newline at end of file diff --git a/kortex_examples/python/PlayCartesian.py b/kortex_examples/python/PlayCartesian.py new file mode 100644 index 00000000..5c243d1f --- /dev/null +++ b/kortex_examples/python/PlayCartesian.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +import sys +import rospy +from kortex_driver.srv import * +from kortex_driver.msg import * + +def PlayCartesian_client(): + + rospy.wait_for_service('PlayCartesianTrajectory') + rospy.wait_for_service('RefreshFeedback') + + try: + function_PlayCartesianTrajectory = rospy.ServiceProxy('PlayCartesianTrajectory', PlayCartesianTrajectory) + function_RefreshFeedback = rospy.ServiceProxy('RefreshFeedback', RefreshFeedback) + + current_feedback = function_RefreshFeedback() + request = PlayCartesianTrajectoryRequest() + + current_x = current_feedback.output.base.tool_pose_x + current_y = current_feedback.output.base.tool_pose_y + current_z = current_feedback.output.base.tool_pose_z + + current_theta_x = current_feedback.output.base.tool_pose_theta_x + current_theta_y = current_feedback.output.base.tool_pose_theta_y + current_theta_z = current_feedback.output.base.tool_pose_theta_z + + //Creating our next target (a Cartesian pose) + request.input.target_pose.x = current_x + request.input.target_pose.y = current_y + request.input.target_pose.z = current_z + 0.1 + + request.input.target_pose.theta_x = current_theta_x + request.input.target_pose.theta_y = current_theta_y + request.input.target_pose.theta_z = current_theta_z + 60 + + poseSpeed = CartesianSpeed() + poseSpeed.translation = 0.1 + poseSpeed.orientation = 15 + + request.input.constraint.oneof_type.speed.append(poseSpeed) + + function_PlayCartesianTrajectory(request) + + except rospy.ServiceException as e: + print "Service call failed: %s"%e + +if __name__ == "__main__": + PlayCartesian_client() \ No newline at end of file diff --git a/kortex_examples/python/ReadAllDevices.py b/kortex_examples/python/ReadAllDevices.py new file mode 100644 index 00000000..d0adee9b --- /dev/null +++ b/kortex_examples/python/ReadAllDevices.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python + +import sys +import rospy +from kortex_device_manager.srv import * + +def ReadAllDevices_client(): + rospy.wait_for_service('ReadAllDevices') + try: + function_ReadAllDevices = rospy.ServiceProxy('ReadAllDevices', ReadAllDevices) + resp1 = function_ReadAllDevices() + print(resp1) + except rospy.ServiceException as e: + print "Service call failed: %s"%e + + + +if __name__ == "__main__": + ReadAllDevices_client() \ No newline at end of file diff --git a/kortex_examples/python/SetControlLoopParameters.py b/kortex_examples/python/SetControlLoopParameters.py new file mode 100644 index 00000000..7ab80e98 --- /dev/null +++ b/kortex_examples/python/SetControlLoopParameters.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +import sys +import rospy +import time +from kortex_actuator_driver.srv import * +from kortex_actuator_driver.msg import * +from kortex_device_manager.srv import * +from kortex_device_manager.msg import * + +def _function_SetControlLoopParameters(): + rospy.wait_for_service('SetControlLoopParameters') + + try: + function_SetControlLoopParameters = rospy.ServiceProxy('SetControlLoopParameters', SetControlLoopParameters) + function_SetDeviceID = rospy.ServiceProxy('SetDeviceID', SetDeviceID) + function_ReadAllDevices = rospy.ServiceProxy('ReadAllDevices', ReadAllDevices) + + req = SetControlLoopParametersRequest() + reqReadAllDevices = ReadAllDevicesRequest() + reqSetDeviceID = SetDeviceIDRequest() + + req.input.loop_selection = ControlLoopSelection.MOTOR_VELOCITY; + + actuatorFound = False; + actuator_id = 0; + + responseReadAllDevices = function_ReadAllDevices(reqReadAllDevices) + + for index, device in enumerate(responseReadAllDevices.output.device_handle): + if device.device_type is DeviceTypes.BIG_ACTUATOR or device.device_type is DeviceTypes.SMALL_ACTUATOR: + actuator_id = device.device_identifier + actuatorFound = True + reqSetDeviceID.device_id = actuator_id + function_SetDeviceID(reqSetDeviceID) + break + + if actuatorFound: + function_SetControlLoopParameters(req) + + except rospy.ServiceException as e: + print "Service call failed: %s"%e + +if __name__ == "__main__": + _function_SetControlLoopParameters() \ No newline at end of file diff --git a/kortex_examples/readme.md b/kortex_examples/readme.md new file mode 100644 index 00000000..eb003f84 --- /dev/null +++ b/kortex_examples/readme.md @@ -0,0 +1,112 @@ + +# Kortex Examples + + + +1. [Get control loop parameters](#get-control-loop-parameters) +1. [Set control loop parameters](#set-control-loop-parameters) +1. [Play cartesian](#play-cartesian) +1. [Play cartesian position](#play-cartesian-position) +1. [Get sensor settings](#get-sensor-settings) +1. [Read all devices](#read-all-devices) + + + + + +## Get control loop parameters +

+Gets the control loop parameters from an actuator that is part of a Gen3 robot. +

+ +To run this example, those node need to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) +> - kortex\_actuator\_driver (**rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100**) + +\* Note here that the address **192.168.1.10** is the default IP address of a robot but you can put any IP address that suits your need. + +To run the example: + +rosrun kortex_examples GetControlLoopParameters + + +## Set control loop parameters +

+Sets the control loop parameters from an actuator that is part of a Gen3 robot. +

+ +This example needs those node to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) +> - kortex\_actuator\_driver (**rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100**) + +\* Note here that the address **192.168.1.10** is the default IP address of a robot but you can put any IP address that suits your need. + + +## Play cartesian +

+Move the end effector of a Gen3 robot in the Z axix by +0,1 meter and rotate around the THETA Z by +60°. +

+ +To run this example, those node need to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) +> - kortex\_actuator\_driver (**rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100**) + +\* Note here that the address **192.168.1.10** is the default IP address of a robot but you can put any IP address that suits your need. + +To run the example: + +rosrun kortex_examples PlayCartesian + + +## Play cartesian position + +

+Move the end effector of a Gen3 robot in the Z axix by +0,1. +

+ +To run this example, those node need to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) +> - kortex\_actuator\_driver (**rosrun kortex\_actuator\_driver kortex\_actuator\_driver 192.168.1.10 100**) + +\* Note here that the address **192.168.1.10** is the default IP address of a robot but you can put any IP address that suits your need. + +To run the example: + +rosrun kortex_examples PlayCartesianPosition + + +## Get sensor settings +

+ Gets the settings of all the sensors from a vision module. In this example, we assume that the targeted vision module is part of a robot and to you want to communicate with through the robot's base using the device routing feature. +

+ +To run this example, those node need to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) +> - kortex\_vision\_config\_driver (**rosrun kortex\_vision\_config\_driver kortex\_vision\_config\_driver 192.168.1.10**) + +Then to run it: + +rosrun kortex_examples GetSensorSettings + + +## Read all devices +

+ Get a list of all the devices available on a Gen3 robot's base. This example is useful when you want to communicate with a device that is a part of a robot using the device routin feature. +

+ +To run this example, those node need to be running: +> - kortex\_device\_manager (**rosrun kortex\_device\_manager kortex\_device\_manager 192.168.1.10**) + +Then to run it: + +rosrun kortex_examples ReadAllDevices + + diff --git a/kortex_vision_config_driver/CMakeLists.txt b/kortex_vision_config_driver/CMakeLists.txt new file mode 100644 index 00000000..a043a07c --- /dev/null +++ b/kortex_vision_config_driver/CMakeLists.txt @@ -0,0 +1,41 @@ + +cmake_minimum_required(VERSION 2.8.3) +project(kortex_vision_config_driver) + +add_compile_options(-std=c++11) +add_definitions(-D_OS_UNIX) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) + +file(GLOB_RECURSE cpp_list RELATIVE ${PROJECT_SOURCE_DIR} "src/*.cpp") + +## Declare ROS messages and services +add_message_files(DIRECTORY msg) +add_message_files(DIRECTORY msg/non_generated) +add_service_files(DIRECTORY srv) +add_service_files(DIRECTORY srv/non_generated) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${PROJECT_SOURCE_DIR}/src) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/client_stubs) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/messages) +include_directories(include ${PROJECT_SOURCE_DIR}/../kortex_api/include/common) +include_directories(include ${PROJECT_SOURCE_DIR}/src/util) + +link_directories(${PROJECT_SOURCE_DIR}/../kortex_api/lib/release) + +add_executable(${PROJECT_NAME} ${cpp_list}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} CppKinovaApi gcov) + +add_dependencies(${PROJECT_NAME} kortex_vision_config_driver_gencpp) + + diff --git a/kortex_vision_config_driver/RosGeneration.py b/kortex_vision_config_driver/RosGeneration.py new file mode 100644 index 00000000..f47a0d01 --- /dev/null +++ b/kortex_vision_config_driver/RosGeneration.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python + +import sys + +from google.protobuf.compiler import plugin_pb2 as plugin +from google.protobuf import json_format as json_f + +import jinja2 + +import itertools +import json +import types +import os +import sys + +from google.protobuf.descriptor_pb2 import DescriptorProto, EnumDescriptorProto, ServiceDescriptorProto, FieldDescriptorProto, OneofDescriptorProto + +#Class that holds a protobuf message and some other details needed by the generator(jinja2 template). +class DetailedMessage: + def __init__(self, message=None): + self.message = message + self.HasOneOf = "false" + self.oneOfList = [] + +#Class that holds a protobuf service and some other details needed by the generator(jinja2 template). +class DetailedPackage: + def __init__(self, service=None): + self.name = "NoName" + self.service = service + +#JINJA2 function to render a file from a template. +def render(tpl_path, context): + path, filename = os.path.split(tpl_path) + return jinja2.Environment(loader=jinja2.FileSystemLoader(path or './')).get_template(filename).render(**context) + +#Main plugin function +def generate_code(request, response): + + #The context is the object sent to the JINJA2 template + context = types.SimpleNamespace() + context.serviceVersion = 1 + + context.detailedPackages = [] + + MainFilePath = os.path.join(".", "src/main.cpp") + function_list = [] + fileIndex = 0 + + for proto_file in request.proto_file: + context.detailedPackages.append(DetailedPackage()) + context.detailedPackages[fileIndex].name = proto_file.package.split(".")[-1] + context.detailedPackages[fileIndex].filename = proto_file.name.split(".")[0] + context.detailedPackages[fileIndex].namespace = proto_file.package.replace(".", "::") + context.detailedPackages[fileIndex].HasRPC = 0 + context.detailedPackages[fileIndex].HasMessage = 0 + + HeaderFilePath = os.path.join(".", "src/node.h") + CppFilePath = os.path.join(".", "src/node.cpp") + + #We lower the case to respect ROS coding standard style + CppProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderProtoConverterFilePath = os.path.join(".", "src/{}_proto_converter.h".format(proto_file.name.split(".")[0].lower())) + CppRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.cpp".format(proto_file.name.split(".")[0].lower())) + HeaderRosConverterFilePath = os.path.join(".", "src/{}_ros_converter.h".format(proto_file.name.split(".")[0].lower())) + + list_detailedMessage = [] + list_detailedMethod = [] + + # For every item in the current proto file + for item, package in traverse(proto_file): + context.HasOneOf = 0 + + + if isinstance(item, EnumDescriptorProto): + context.item = item + + ros_enumPath = os.path.join(".", "msg/{}.msg".format(item.name)) + + with open(ros_enumPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_enum.jinja2", context.__dict__)) + #If this it a message + if isinstance(item, DescriptorProto): + tempMessage = DetailedMessage(item) + context.detailedPackages[fileIndex].HasMessage = 1 + + for member in item.field: + #If a member is part of a oneof, it will have this additional field + if member.HasField("oneof_index"): + context.HasOneOf = 1 + tempMessage.HasOneOf = "true" + else: + context.HasOneOf = 0 + tempMessage.HasOneOf = "false" + + context.item = item + + #If the proto file contains a ONEOF we need to generate a separate file to handle it. + if context.HasOneOf == 1: + + #This line gets the list of ONEOF that is in the current message. + oneOfList = item.ListFields()[-1][1] + + tempMessage.oneOfList = item.ListFields()[-1][1] + ros_oneofPath = os.path.join(".", "msg/{}_{}.msg".format(item.name, oneOfList[0].name)) + + with open(ros_oneofPath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_oneof.jinja2", context.__dict__)) + + + list_detailedMessage.append(tempMessage) + ros_messagePath = os.path.join(".", "msg/{}.msg".format(item.name)) + + #We call jinja2 to generate a ROS message. + with open(ros_messagePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_message.jinja2", context.__dict__)) + + #If this is a service (A group of method) + if isinstance(item, ServiceDescriptorProto): + for method in item.method: + context.item = method + if "Topic" not in method.name: + function_list.append(method.name) + ros_servicePath = os.path.join(".", "srv/{}.srv".format(method.name)) + with open(ros_servicePath, 'wt') as serviceFile: + serviceFile.write(render("./templates/ros_service.jinja2", context.__dict__)) + + context.detailedPackages[fileIndex].service = item + context.detailedPackages[fileIndex].HasRPC = 1 + + context.currentPackageName = context.detailedPackages[fileIndex].name + context.currentNamespace = proto_file.package.replace(".", "::") + context.currentFilename = context.detailedPackages[fileIndex].filename + context.item = list_detailedMessage + + if context.detailedPackages[fileIndex].HasMessage == 1: + #Wecall jinja2 to generate a prot/ROS converter for every protobuf message. + with open(CppProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.cpp.jinja2", context.__dict__)) + with open(HeaderProtoConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/proto_converter.h.jinja2", context.__dict__)) + with open(CppRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.cpp.jinja2", context.__dict__)) + with open(HeaderRosConverterFilePath, 'wt') as converterFile: + converterFile.write(render("./templates/ros_converter.h.jinja2", context.__dict__)) + + fileIndex = fileIndex + 1 + + context.list_function = function_list + + #We jinja2 to generate the ROS node. + with open(HeaderFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.h.jinja2", context.__dict__)) + with open(CppFilePath, 'wt') as nodeFile: + nodeFile.write(render("./templates/NodeServices.cpp.jinja2", context.__dict__)) + with open(MainFilePath, 'wt') as mainFile: + mainFile.write(render("./templates/main.jinja2", context.__dict__)) + +def traverse(proto_file): + #recursive function that browse a protobof item + def _traverse(package, items): + for item in items: + yield item, package + + if isinstance(item, DescriptorProto): + for enum in item.enum_type: + yield enum, package + + for nested in item.nested_type: + nested_package = package + item.name + + for nested_item in _traverse(nested, nested_package): + yield nested_item, nested_package + if isinstance(item, ServiceDescriptorProto): + for rpc in item.method: + yield rpc, package + + #return a list of everything found in the proto file + return itertools.chain( + _traverse(proto_file.package, proto_file.enum_type), + _traverse(proto_file.package, proto_file.message_type), + _traverse(proto_file.package, proto_file.service), + ) + +if __name__ == '__main__': + # Read request message from stdin + data = sys.stdin.buffer.read() + + # Parse request + request = plugin.CodeGeneratorRequest() + request.ParseFromString(data) + + # Create response + response = plugin.CodeGeneratorResponse() + + # Generate code + generate_code(request, response) + + # Serialise response message + output = response.SerializeToString() + + # Write to stdout + sys.stdout.buffer.write(output) \ No newline at end of file diff --git a/kortex_vision_config_driver/build/.gitignore b/kortex_vision_config_driver/build/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/kortex_vision_config_driver/kortex_vision_config_driver.sh b/kortex_vision_config_driver/kortex_vision_config_driver.sh new file mode 100755 index 00000000..011eee96 --- /dev/null +++ b/kortex_vision_config_driver/kortex_vision_config_driver.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +python3 -u RosGeneration.py + diff --git a/kortex_vision_config_driver/msg/ArmState.msg b/kortex_vision_config_driver/msg/ArmState.msg new file mode 100644 index 00000000..d85bd69d --- /dev/null +++ b/kortex_vision_config_driver/msg/ArmState.msg @@ -0,0 +1,22 @@ + +uint32 UNSPECIFIED_ARM_STATE = 0 + +uint32 BASE_INITIALIZATION = 1 + +uint32 IDLE = 2 + +uint32 ARM_INITIALIZATION = 3 + +uint32 ARM_IN_FAULT = 4 + +uint32 ARM_MAINTENANCE = 5 + +uint32 ARM_SERVOING_LOW_LEVEL = 6 + +uint32 ARM_SERVOING_READY = 7 + +uint32 ARM_SERVOING_PLAYING_SEQUENCE = 8 + +uint32 ARM_SERVOING_MANUALLY_CONTROLLED = 9 + +uint32 RESERVED = 255 diff --git a/kortex_vision_config_driver/msg/BitRate.msg b/kortex_vision_config_driver/msg/BitRate.msg new file mode 100644 index 00000000..49596067 --- /dev/null +++ b/kortex_vision_config_driver/msg/BitRate.msg @@ -0,0 +1,10 @@ + +uint32 BITRATE_UNSPECIFIED = 0 + +uint32 BITRATE_10_MBPS = 1 + +uint32 BITRATE_15_MBPS = 2 + +uint32 BITRATE_20_MBPS = 3 + +uint32 BITRATE_25_MBPS = 4 diff --git a/kortex_vision_config_driver/msg/Connection.msg b/kortex_vision_config_driver/msg/Connection.msg new file mode 100644 index 00000000..ca08b6c8 --- /dev/null +++ b/kortex_vision_config_driver/msg/Connection.msg @@ -0,0 +1,5 @@ + + +UserProfileHandle user_handle +string connection_information +uint32 connection_identifier \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/DeviceHandle.msg b/kortex_vision_config_driver/msg/DeviceHandle.msg new file mode 100644 index 00000000..49f84366 --- /dev/null +++ b/kortex_vision_config_driver/msg/DeviceHandle.msg @@ -0,0 +1,5 @@ + + +uint32 device_type +uint32 device_identifier +uint32 order \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/DeviceTypes.msg b/kortex_vision_config_driver/msg/DeviceTypes.msg new file mode 100644 index 00000000..5a55df07 --- /dev/null +++ b/kortex_vision_config_driver/msg/DeviceTypes.msg @@ -0,0 +1,14 @@ + +uint32 UNSPECIFIED_DEVICE_TYPE = 0 + +uint32 BASE = 1 + +uint32 VISION = 2 + +uint32 BIG_ACTUATOR = 3 + +uint32 SMALL_ACTUATOR = 4 + +uint32 INTERCONNECT = 5 + +uint32 GRIPPER = 6 diff --git a/kortex_vision_config_driver/msg/Empty.msg b/kortex_vision_config_driver/msg/Empty.msg new file mode 100644 index 00000000..e69de29b diff --git a/kortex_vision_config_driver/msg/FocusAction.msg b/kortex_vision_config_driver/msg/FocusAction.msg new file mode 100644 index 00000000..29ba2f8f --- /dev/null +++ b/kortex_vision_config_driver/msg/FocusAction.msg @@ -0,0 +1,10 @@ + +uint32 FOCUSACTION_UNSPECIFIED = 0 + +uint32 FOCUSACTION_START_CONTINUOUS_FOCUS = 1 + +uint32 FOCUSACTION_PAUSE_CONTINUOUS_FOCUS = 2 + +uint32 FOCUSACTION_FOCUS_NOW = 3 + +uint32 FOCUSACTION_DISABLE_FOCUS = 4 diff --git a/kortex_vision_config_driver/msg/FrameRate.msg b/kortex_vision_config_driver/msg/FrameRate.msg new file mode 100644 index 00000000..1f3368d9 --- /dev/null +++ b/kortex_vision_config_driver/msg/FrameRate.msg @@ -0,0 +1,8 @@ + +uint32 FRAMERATE_UNSPECIFIED = 0 + +uint32 FRAMERATE_6_FPS = 1 + +uint32 FRAMERATE_15_FPS = 2 + +uint32 FRAMERATE_30_FPS = 3 diff --git a/kortex_vision_config_driver/msg/IntrinsicParameters.msg b/kortex_vision_config_driver/msg/IntrinsicParameters.msg new file mode 100644 index 00000000..e3455c1a --- /dev/null +++ b/kortex_vision_config_driver/msg/IntrinsicParameters.msg @@ -0,0 +1,7 @@ + +uint32 width +uint32 height +float32 principal_point_x +float32 principal_point_y +float32 focal_length_x +float32 focal_length_y \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/NotificationHandle.msg b/kortex_vision_config_driver/msg/NotificationHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_vision_config_driver/msg/NotificationHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/NotificationOptions.msg b/kortex_vision_config_driver/msg/NotificationOptions.msg new file mode 100644 index 00000000..6ff46842 --- /dev/null +++ b/kortex_vision_config_driver/msg/NotificationOptions.msg @@ -0,0 +1,5 @@ + + +uint32 type +uint32 rate_m_sec +float32 threshold_value \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/NotificationType.msg b/kortex_vision_config_driver/msg/NotificationType.msg new file mode 100644 index 00000000..79dd058c --- /dev/null +++ b/kortex_vision_config_driver/msg/NotificationType.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED_NOTIFICATION_TYPE = 0 + +uint32 THRESHOLD = 1 + +uint32 FIX_RATE = 2 + +uint32 EVENT = 3 diff --git a/kortex_vision_config_driver/msg/Option.msg b/kortex_vision_config_driver/msg/Option.msg new file mode 100644 index 00000000..aa509db1 --- /dev/null +++ b/kortex_vision_config_driver/msg/Option.msg @@ -0,0 +1,86 @@ + +uint32 OPTION_UNSPECIFIED = 0 + +uint32 OPTION_BACKLIGHT_COMPENSATION = 1 + +uint32 OPTION_BRIGHTNESS = 2 + +uint32 OPTION_CONTRAST = 3 + +uint32 OPTION_EXPOSURE = 4 + +uint32 OPTION_GAIN = 5 + +uint32 OPTION_GAMMA = 6 + +uint32 OPTION_HUE = 7 + +uint32 OPTION_SATURATION = 8 + +uint32 OPTION_SHARPNESS = 9 + +uint32 OPTION_WHITE_BALANCE = 10 + +uint32 OPTION_ENABLE_AUTO_EXPOSURE = 11 + +uint32 OPTION_ENABLE_AUTO_WHITE_BALANCE = 12 + +uint32 OPTION_VISUAL_PRESET = 13 + +uint32 OPTION_LASER_POWER = 14 + +uint32 OPTION_ACCURACY = 15 + +uint32 OPTION_MOTION_RANGE = 16 + +uint32 OPTION_FILTER_OPTION = 17 + +uint32 OPTION_CONFIDENCE_THRESHOLD = 18 + +uint32 OPTION_EMITTER_ENABLED = 19 + +uint32 OPTION_FRAMES_QUEUE_SIZE = 20 + +uint32 OPTION_TOTAL_FRAME_DROPS = 21 + +uint32 OPTION_AUTO_EXPOSURE_MODE = 22 + +uint32 OPTION_POWER_LINE_FREQUENCY = 23 + +uint32 OPTION_ASIC_TEMPERATURE = 24 + +uint32 OPTION_ERROR_POLLING_ENABLED = 25 + +uint32 OPTION_PROJECTOR_TEMPERATURE = 26 + +uint32 OPTION_OUTPUT_TRIGGER_ENABLED = 27 + +uint32 OPTION_MOTION_MODULE_TEMPERATURE = 28 + +uint32 OPTION_DEPTH_UNITS = 29 + +uint32 OPTION_ENABLE_MOTION_CORRECTION = 30 + +uint32 OPTION_AUTO_EXPOSURE_PRIORITY = 31 + +uint32 OPTION_COLOR_SCHEME = 32 + +uint32 OPTION_HISTOGRAM_EQUALIZATION_ENABLED = 33 + +uint32 OPTION_MIN_DISTANCE = 34 + +uint32 OPTION_MAX_DISTANCE = 35 + +uint32 OPTION_TEXTURE_SOURCE = 36 + +uint32 OPTION_FILTER_MAGNITUDE = 37 + +uint32 OPTION_FILTER_SMOOTH_ALPHA = 38 + +uint32 OPTION_FILTER_SMOOTH_DELTA = 39 + +uint32 OPTION_HOLES_FILL = 40 + +uint32 OPTION_STEREO_BASELINE = 41 + +uint32 OPTION_AUTO_EXPOSURE_CONVERGE_STEP = 42 diff --git a/kortex_vision_config_driver/msg/OptionIdentifier.msg b/kortex_vision_config_driver/msg/OptionIdentifier.msg new file mode 100644 index 00000000..188c0269 --- /dev/null +++ b/kortex_vision_config_driver/msg/OptionIdentifier.msg @@ -0,0 +1,5 @@ + + +uint32 sensor + +uint32 option \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/OptionInformation.msg b/kortex_vision_config_driver/msg/OptionInformation.msg new file mode 100644 index 00000000..d89df3ae --- /dev/null +++ b/kortex_vision_config_driver/msg/OptionInformation.msg @@ -0,0 +1,11 @@ + + +uint32 sensor + +uint32 option +bool supported +bool read_only +float32 minimum +float32 maximum +float32 step +float32 default_value \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/OptionValue.msg b/kortex_vision_config_driver/msg/OptionValue.msg new file mode 100644 index 00000000..a69f5305 --- /dev/null +++ b/kortex_vision_config_driver/msg/OptionValue.msg @@ -0,0 +1,6 @@ + + +uint32 sensor + +uint32 option +float32 value \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/Permission.msg b/kortex_vision_config_driver/msg/Permission.msg new file mode 100644 index 00000000..c5399e9c --- /dev/null +++ b/kortex_vision_config_driver/msg/Permission.msg @@ -0,0 +1,8 @@ + +uint32 NO_PERMISSION = 0 + +uint32 READ_PERMISSION = 1 + +uint32 UPDATE_PERMISSION = 2 + +uint32 DELETE_PERMISSION = 4 diff --git a/kortex_vision_config_driver/msg/Resolution.msg b/kortex_vision_config_driver/msg/Resolution.msg new file mode 100644 index 00000000..d0901c9e --- /dev/null +++ b/kortex_vision_config_driver/msg/Resolution.msg @@ -0,0 +1,14 @@ + +uint32 RESOLUTION_UNSPECIFIED = 0 + +uint32 RESOLUTION_320x240 = 1 + +uint32 RESOLUTION_424x240 = 2 + +uint32 RESOLUTION_480x270 = 3 + +uint32 RESOLUTION_640x480 = 4 + +uint32 RESOLUTION_1280x720 = 5 + +uint32 RESOLUTION_1920x1080 = 6 diff --git a/kortex_vision_config_driver/msg/SafetyHandle.msg b/kortex_vision_config_driver/msg/SafetyHandle.msg new file mode 100644 index 00000000..bd72812b --- /dev/null +++ b/kortex_vision_config_driver/msg/SafetyHandle.msg @@ -0,0 +1,2 @@ + +uint32 identifier \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/SafetyNotification.msg b/kortex_vision_config_driver/msg/SafetyNotification.msg new file mode 100644 index 00000000..36d88894 --- /dev/null +++ b/kortex_vision_config_driver/msg/SafetyNotification.msg @@ -0,0 +1,11 @@ + + +SafetyHandle safety_handle + +uint32 value + +Timestamp timestamp + +UserProfileHandle user_handle + +Connection connection \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/SafetyStatusValue.msg b/kortex_vision_config_driver/msg/SafetyStatusValue.msg new file mode 100644 index 00000000..2ea80554 --- /dev/null +++ b/kortex_vision_config_driver/msg/SafetyStatusValue.msg @@ -0,0 +1,8 @@ + +uint32 UNSPECIFIED = 0 + +uint32 WARNING = 1 + +uint32 ERROR = 2 + +uint32 NORMAL = 3 diff --git a/kortex_vision_config_driver/msg/Sensor.msg b/kortex_vision_config_driver/msg/Sensor.msg new file mode 100644 index 00000000..193ad1ff --- /dev/null +++ b/kortex_vision_config_driver/msg/Sensor.msg @@ -0,0 +1,6 @@ + +uint32 SENSOR_UNSPECIFIED = 0 + +uint32 SENSOR_COLOR = 1 + +uint32 SENSOR_DEPTH = 2 diff --git a/kortex_vision_config_driver/msg/SensorFocusAction.msg b/kortex_vision_config_driver/msg/SensorFocusAction.msg new file mode 100644 index 00000000..c5db660d --- /dev/null +++ b/kortex_vision_config_driver/msg/SensorFocusAction.msg @@ -0,0 +1,5 @@ + + +uint32 sensor + +uint32 focus_action \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/SensorIdentifier.msg b/kortex_vision_config_driver/msg/SensorIdentifier.msg new file mode 100644 index 00000000..a6ea8c96 --- /dev/null +++ b/kortex_vision_config_driver/msg/SensorIdentifier.msg @@ -0,0 +1,3 @@ + + +uint32 sensor \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/SensorSettings.msg b/kortex_vision_config_driver/msg/SensorSettings.msg new file mode 100644 index 00000000..7225b43b --- /dev/null +++ b/kortex_vision_config_driver/msg/SensorSettings.msg @@ -0,0 +1,9 @@ + + +uint32 sensor + +uint32 resolution + +uint32 frame_rate + +uint32 bit_rate \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/ServiceVersion.msg b/kortex_vision_config_driver/msg/ServiceVersion.msg new file mode 100644 index 00000000..9665d1c2 --- /dev/null +++ b/kortex_vision_config_driver/msg/ServiceVersion.msg @@ -0,0 +1,4 @@ + +uint32 RESERVED_0 = 0 + +uint32 CURRENT_VERSION = 1 diff --git a/kortex_vision_config_driver/msg/Timestamp.msg b/kortex_vision_config_driver/msg/Timestamp.msg new file mode 100644 index 00000000..5e60508c --- /dev/null +++ b/kortex_vision_config_driver/msg/Timestamp.msg @@ -0,0 +1,3 @@ + +uint32 sec +uint32 usec \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/Unit.msg b/kortex_vision_config_driver/msg/Unit.msg new file mode 100644 index 00000000..c019aeba --- /dev/null +++ b/kortex_vision_config_driver/msg/Unit.msg @@ -0,0 +1,28 @@ + +uint32 UNSPECIFIED_UNIT = 0 + +uint32 CELSIUS = 1 + +uint32 AMPERE = 2 + +uint32 VOLT = 3 + +uint32 METER_PER_SECOND = 4 + +uint32 DEGREE_PER_SECOND = 5 + +uint32 METER_PER_SECOND_2 = 6 + +uint32 DEGREE_PER_SECOND_2 = 7 + +uint32 NEWTON = 8 + +uint32 NEWTON_METER = 9 + +uint32 KILOGRAM = 10 + +uint32 DEGREE = 11 + +uint32 TICK = 12 + +uint32 DEGREE_PER_MILLISECOND = 13 diff --git a/kortex_vision_config_driver/msg/UserProfileHandle.msg b/kortex_vision_config_driver/msg/UserProfileHandle.msg new file mode 100644 index 00000000..3038380d --- /dev/null +++ b/kortex_vision_config_driver/msg/UserProfileHandle.msg @@ -0,0 +1,3 @@ + +uint32 identifier +uint32 permission \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/VisionEvent.msg b/kortex_vision_config_driver/msg/VisionEvent.msg new file mode 100644 index 00000000..65d7bd41 --- /dev/null +++ b/kortex_vision_config_driver/msg/VisionEvent.msg @@ -0,0 +1,6 @@ + +uint32 UNSPECIFIED_VISION_EVENT = 0 + +uint32 SENSOR_SETTINGS_CHANGED = 1 + +uint32 OPTION_VALUE_CHANGED = 2 diff --git a/kortex_vision_config_driver/msg/VisionNotification.msg b/kortex_vision_config_driver/msg/VisionNotification.msg new file mode 100644 index 00000000..79f43120 --- /dev/null +++ b/kortex_vision_config_driver/msg/VisionNotification.msg @@ -0,0 +1,7 @@ + + +uint32 event + +uint32 sensor + +uint32 option \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/non_generated/ApiOptions.msg b/kortex_vision_config_driver/msg/non_generated/ApiOptions.msg new file mode 100644 index 00000000..471fddd8 --- /dev/null +++ b/kortex_vision_config_driver/msg/non_generated/ApiOptions.msg @@ -0,0 +1 @@ +uint32 timeout_ms \ No newline at end of file diff --git a/kortex_vision_config_driver/msg/non_generated/KortexError.msg b/kortex_vision_config_driver/msg/non_generated/KortexError.msg new file mode 100644 index 00000000..3ff9c32c --- /dev/null +++ b/kortex_vision_config_driver/msg/non_generated/KortexError.msg @@ -0,0 +1,3 @@ +uint32 code +uint32 subCode +string description \ No newline at end of file diff --git a/kortex_vision_config_driver/package.xml b/kortex_vision_config_driver/package.xml new file mode 100644 index 00000000..b4ede718 --- /dev/null +++ b/kortex_vision_config_driver/package.xml @@ -0,0 +1,35 @@ + + + kortex_vision_config_driver + 1.0.0 + The kortex package that configure a vision module. + + + KINOVA + + + + + + BSD + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_generation + message_runtime + + + + + + + + diff --git a/kortex_vision_config_driver/protos/Common.options b/kortex_vision_config_driver/protos/Common.options new file mode 100644 index 00000000..5f3020ab --- /dev/null +++ b/kortex_vision_config_driver/protos/Common.options @@ -0,0 +1,4 @@ +Kinova.Api.Common.DeviceTypes long_names:false +Kinova.Api.Common.SafetyStatusValue long_names:false +Kinova.Api.Common.NotificationType long_names:false +Kinova.Api.Common.Unit long_names:false \ No newline at end of file diff --git a/kortex_vision_config_driver/protos/Common.proto b/kortex_vision_config_driver/protos/Common.proto new file mode 100644 index 00000000..50f762ec --- /dev/null +++ b/kortex_vision_config_driver/protos/Common.proto @@ -0,0 +1,152 @@ +syntax = "proto3"; + +package Kinova.Api.Common; + +/** + * list of possible device types + */ +enum DeviceTypes { + UNSPECIFIED_DEVICE_TYPE = 0; + BASE = 1; + VISION = 2; + BIG_ACTUATOR = 3; + SMALL_ACTUATOR = 4; + INTERCONNECT = 5; + GRIPPER = 6; +} + +/** +* Message contains information about a device - device type, device identifier, and the order of the device within the robot +*/ +message DeviceHandle { + DeviceTypes device_type = 1; + uint32 device_identifier = 2; // Unique device identifier (used with other services) + uint32 order = 3; // Unique value indicating the order of that device versus the others to facilitate representation +} + +/** + * list of possible safety statuses + */ +enum SafetyStatusValue { + UNSPECIFIED = 0; + WARNING = 1; //warning safety reached + ERROR = 2; //error safety reached + NORMAL = 3; //safety is off +} + +/** + * Enumeration used as bitfields wih permission field + */ +enum Permission { + NO_PERMISSION = 0; + READ_PERMISSION = 1; //refers to a user's capability to read the entity + UPDATE_PERMISSION = 2;//refers to a user's capability to write or modify the entity + DELETE_PERMISSION = 4; //refers to a user's capability to delete the entity +} + +/** + * list of notification types + */ +enum NotificationType { + UNSPECIFIED_NOTIFICATION_TYPE = 0; + THRESHOLD = 1; + FIX_RATE = 2; + EVENT = 3; //Event type. Only this one is supported for now +} + +/** + * list of units used throughout API methods + */ +enum Unit { + UNSPECIFIED_UNIT = 0; + CELSIUS = 1; + AMPERE = 2; + VOLT = 3; + METER_PER_SECOND = 4; + DEGREE_PER_SECOND = 5; + METER_PER_SECOND_2 = 6; + DEGREE_PER_SECOND_2 = 7; + NEWTON = 8; + NEWTON_METER =9; + KILOGRAM = 10; + DEGREE = 11; + TICK = 12; + DEGREE_PER_MILLISECOND = 13; +} + +/** + * Message used when no information needs to be exchanged between client application and robot, and vice versa + */ +message Empty { +} + +/** + * Notification options + */ +message NotificationOptions { + NotificationType type = 1; //type of notification + uint32 rate_m_sec = 2; + float threshold_value = 3; +} + +/** + * Handle to a safety + */ +message SafetyHandle { + uint32 identifier = 1; +} + +/** + * Handle to a notification + */ +message NotificationHandle { + uint32 identifier = 1; +} + +/** + * Message that contains a Safety event + */ +message SafetyNotification { + SafetyHandle safety_handle = 1; //safety handle + SafetyStatusValue value = 2; //new safety status + Timestamp timestamp = 3; //event timestamp + UserProfileHandle user_handle = 4; //user that caused the safety event + Connection connection = 5; // connection that caused the safety event +} + +/** + * Timestamp based on epoch + */ +message Timestamp { + uint32 sec = 1; //epoch in seconds since 1970 + uint32 usec = 2;//microseconds after the second (0-999999) +} + +/** + * Handle to an existing User Profile. + */ +message UserProfileHandle { + uint32 identifier = 1; //User profile identifier + fixed32 permission = 2; //must use 'Permission' as bitwise +} + +message Connection { + UserProfileHandle user_handle = 1; //user profile handle, or set to zero if no user logged in + string connection_information = 2; //connection info (ex. IP address with port number) + uint32 connection_identifier = 3; //connection identifier +} + +enum ArmState +{ + UNSPECIFIED_ARM_STATE = 0; + BASE_INITIALIZATION = 1; // Cannot be reported as the Base initialization must be completed before allowing user connection + IDLE = 2; + ARM_INITIALIZATION = 3; + ARM_IN_FAULT = 4; + ARM_MAINTENANCE = 5; + ARM_SERVOING_LOW_LEVEL = 6; + ARM_SERVOING_READY = 7; + ARM_SERVOING_PLAYING_SEQUENCE = 8; + ARM_SERVOING_MANUALLY_CONTROLLED = 9; + RESERVED = 255; // For debugging, this state must never be reported outside the base. this means that a state is not mapped correctly +} \ No newline at end of file diff --git a/kortex_vision_config_driver/protos/VisionConfig.proto b/kortex_vision_config_driver/protos/VisionConfig.proto new file mode 100644 index 00000000..886ff536 --- /dev/null +++ b/kortex_vision_config_driver/protos/VisionConfig.proto @@ -0,0 +1,217 @@ +syntax = "proto3"; + +import public "Common.proto"; + +package Kinova.Api.VisionConfig; + +service VisionConfig {//@PROXY_ID=5 @ERROR=Kinova.Api.Error + + // Set sensor settings (resolution, frame rate, etc) + rpc SetSensorSettings (SensorSettings) returns (Kinova.Api.Common.Empty);//@RPC_ID=1 + + // Get sensor settings (resolution, frame rate, etc) + rpc GetSensorSettings (SensorIdentifier) returns (SensorSettings);//@RPC_ID=2 + + // Read option value from the sensor + rpc GetOptionValue (OptionIdentifier) returns (OptionValue);//@RPC_ID=3 + + // Write new value to sensor option + rpc SetOptionValue (OptionValue) returns (Kinova.Api.Common.Empty);//@RPC_ID=4 + + // Read option information from the sensor + rpc GetOptionInformation (OptionIdentifier) returns (OptionInformation);//@RPC_ID=5 + + //Subscribes to Vision configuration notifications + rpc VisionTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=6 @PUB_SUB=VisionNotification + + // Do a focus action + rpc DoSensorFocusAction (SensorFocusAction) returns (Kinova.Api.Common.Empty);//@RPC_ID=7 + + // Get sensor intrinsic parameters + rpc GetIntrinsicParameters (SensorIdentifier) returns (IntrinsicParameters);//@RPC_ID=8 +} + +enum ServiceVersion { + RESERVED_0 = 0; + CURRENT_VERSION = 1; // Current Version +} + +/** +* Main settings - resolution, frame rate, bit rate - for the chosen sensor (color or depth). +*/ +message SensorSettings { + Sensor sensor = 1; // The sensor (color or depth) + Resolution resolution = 2; // The resolution setting + FrameRate frame_rate = 3; // Frame rate setting + BitRate bit_rate = 4; // Maximum encoded bit rate +} + +/** +*Select the Vision module camera sensor to configure. +*/ +enum Sensor { + SENSOR_UNSPECIFIED = 0; + SENSOR_COLOR = 1; //Select the Vision module color sensor + SENSOR_DEPTH = 2; //Select the Vision module depth sensor +} + +/** +*Select the camera resolution. +*/ +enum Resolution { + RESOLUTION_UNSPECIFIED = 0; + RESOLUTION_320x240 = 1; // 320 x 240 pixels + RESOLUTION_424x240 = 2; // 424 x 240 pixels + RESOLUTION_480x270 = 3; // 480 x 270 pixels + RESOLUTION_640x480 = 4; // 640 x 480 pixels + RESOLUTION_1280x720 = 5; // 1280 x 720 pixels (HD) + RESOLUTION_1920x1080 = 6; // 1920 x 1080 pixels (full HD) +} + +/** +*Select the camera frame rate. +*/ +enum FrameRate { + FRAMERATE_UNSPECIFIED = 0; + FRAMERATE_6_FPS = 1; // 6 frames per second + FRAMERATE_15_FPS = 2; // 15 frames per second + FRAMERATE_30_FPS = 3; // 30 frame per second +} + +/** +*Select the maximum encoded bit rate, in Mbps. +*/ +enum BitRate { + BITRATE_UNSPECIFIED = 0; + BITRATE_10_MBPS = 1; // 10 Mbps maximum bit rate + BITRATE_15_MBPS = 2; // 15 Mbps maximum bit rate + BITRATE_20_MBPS = 3; // 20 Mbps maximum bit rate + BITRATE_25_MBPS = 4; // 25 Mbps maximum bit rate +} + +message SensorIdentifier { + Sensor sensor = 1; +} + +/** +* Identifies the sensor and the option to configure. +*/ +message OptionIdentifier { + Sensor sensor = 1; // The sensor to configure + Option option = 2; // The option to configure on the sensor +} + +/** +* Identifies the value of the particular option for the sensor. +*/ +message OptionValue { + Sensor sensor = 1; // The sensor to configure (color or depth) + Option option = 2; // The option to configure + float value = 3; // The desired value for the option +} + +/** +* Packages information about the optional settings for the chosen sensor +*/ +message OptionInformation { + Sensor sensor = 1; // The sensor (color or depth) + Option option = 2; // The option + bool supported = 3; // Is the option supported by the chosen sensor? + bool read_only = 4; // Is the option read-only, or can it be changed? + float minimum = 5; // Minimum value for the option + float maximum = 6; // Maximum value for the option + float step = 7; // Step size for the option value (if it takes on discrete values) + float default_value = 8; // Default value for the option +} + +enum Option { + OPTION_UNSPECIFIED = 0; + OPTION_BACKLIGHT_COMPENSATION = 1; // Enable / disable color backlight compensation + OPTION_BRIGHTNESS = 2; // Color image brightness + OPTION_CONTRAST = 3; // Color image contrast + OPTION_EXPOSURE = 4; // Controls exposure time of color camera. Setting any value will disable auto exposure + OPTION_GAIN = 5; // Color image gain + OPTION_GAMMA = 6; // Color image gamma setting + OPTION_HUE = 7; // Color image hue + OPTION_SATURATION = 8; // Color image saturation setting + OPTION_SHARPNESS = 9; // Color image sharpness setting + OPTION_WHITE_BALANCE = 10; // Controls white balance of color image. Setting any value will disable auto white balance + OPTION_ENABLE_AUTO_EXPOSURE = 11; // Enable / disable color image auto-exposure + OPTION_ENABLE_AUTO_WHITE_BALANCE = 12; // Enable / disable color image auto-white-balance + OPTION_VISUAL_PRESET = 13; // Provide access to several recommend sets of option presets for the depth camera + OPTION_LASER_POWER = 14; // Power of the projector, with 0 meaning projector off + OPTION_ACCURACY = 15; // Set the number of patterns projected per frame. The higher the accuracy value the more patterns projected + OPTION_MOTION_RANGE = 16; // Motion vs. Range trade-off, with lower values allowing for better motion sensitivity and higher values allowing for better depth range + OPTION_FILTER_OPTION = 17; // Set the filter to apply to each depth frame. Each one of the filter is optimized per the application requirements + OPTION_CONFIDENCE_THRESHOLD = 18; // The confidence level threshold used by the Depth algorithm pipe to set whether a pixel will get a valid range or will be marked with invalid range + OPTION_EMITTER_ENABLED = 19; // Laser Emitter enabled + OPTION_FRAMES_QUEUE_SIZE = 20; // Number of frames the user is allowed to keep per stream. Trying to hold-on to more frames will cause frame-drops. + OPTION_TOTAL_FRAME_DROPS = 21; // Total number of detected frame drops from all streams + OPTION_AUTO_EXPOSURE_MODE = 22; // Auto-Exposure modes: Static, Anti-Flicker and Hybrid + OPTION_POWER_LINE_FREQUENCY = 23; // Power Line Frequency control for anti-flickering Off/50Hz/60Hz/Auto + OPTION_ASIC_TEMPERATURE = 24; // Current Asic Temperature + OPTION_ERROR_POLLING_ENABLED = 25; // Disable error handling + OPTION_PROJECTOR_TEMPERATURE = 26; // Current Projector Temperature + OPTION_OUTPUT_TRIGGER_ENABLED = 27; // Enable / disable trigger to be outputed from the camera to any external device on every depth frame + OPTION_MOTION_MODULE_TEMPERATURE = 28; // Current Motion-Module Temperature + OPTION_DEPTH_UNITS = 29; // Number of meters represented by a single depth unit + OPTION_ENABLE_MOTION_CORRECTION = 30; // Enable/Disable automatic correction of the motion data + OPTION_AUTO_EXPOSURE_PRIORITY = 31; // Allows sensor to dynamically ajust the frame rate depending on lighting conditions + OPTION_COLOR_SCHEME = 32; // Color scheme for data visualization + OPTION_HISTOGRAM_EQUALIZATION_ENABLED = 33; // Perform histogram equalization post-processing on the depth data + OPTION_MIN_DISTANCE = 34; // Minimal distance to the target + OPTION_MAX_DISTANCE = 35; // Maximum distance to the target + OPTION_TEXTURE_SOURCE = 36; // Texture mapping stream unique ID + OPTION_FILTER_MAGNITUDE = 37; // The 2D-filter effect. The specific interpretation is given within the context of the filter. + OPTION_FILTER_SMOOTH_ALPHA = 38; // 2D-filter parameter controls the weight/radius for smoothing + OPTION_FILTER_SMOOTH_DELTA = 39; // 2D-filter range/validity threshold + OPTION_HOLES_FILL = 40; // Enhance depth data post-processing with holes filling where appropriate + OPTION_STEREO_BASELINE = 41; // The distance in mm between the first and the second imagers in stereo-based depth cameras + OPTION_AUTO_EXPOSURE_CONVERGE_STEP = 42; // Allows dynamically ajust the converge step value of the target exposure in Auto-Exposure algorithm +} + +/** +* Identifies the sensor and the focus action to perform. +*/ +message SensorFocusAction { + Sensor sensor = 1; // The sensor on which to perform the focus action + FocusAction focus_action = 2; // The focus action to perform on the sensor +} + +/** +*Select the focus action to perform (start/pause continuous, focus now, disable). +*/ +enum FocusAction { + FOCUSACTION_UNSPECIFIED = 0; + FOCUSACTION_START_CONTINUOUS_FOCUS = 1; // Start continuous focus + FOCUSACTION_PAUSE_CONTINUOUS_FOCUS = 2; // Pause continuous focus + FOCUSACTION_FOCUS_NOW = 3; // Focus now (single-shot) + FOCUSACTION_DISABLE_FOCUS = 4; // Disable focus +} + +enum VisionEvent { + UNSPECIFIED_VISION_EVENT = 0; + SENSOR_SETTINGS_CHANGED = 1; + OPTION_VALUE_CHANGED = 2; +} + +/** + * Message that contains robot event + */ +message VisionNotification { + VisionEvent event = 1; + Sensor sensor = 2; + Option option = 3; +} + +/** +* Sensor intrinsic parameters +*/ +message IntrinsicParameters { + uint32 width = 1; // Width of the image in pixels + uint32 height = 2; // Height of the image in pixels + float principal_point_x = 3; // Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge + float principal_point_y = 4; // Vertical coordinate of the principal point of the image, as a pixel offset from the top edge + float focal_length_x = 5; // Focal length of the image plane, as a multiple of pixel width + float focal_length_y = 6; // Focal length of the image plane, as a multiple of pixel height +} diff --git a/kortex_vision_config_driver/readme.md b/kortex_vision_config_driver/readme.md new file mode 100644 index 00000000..b39f21fd --- /dev/null +++ b/kortex_vision_config_driver/readme.md @@ -0,0 +1,106 @@ + +# Kortex Vision Config Module + + + +1. [Content](#content) + 1. [build](#build) + 1. [msg](#msg) + 1. [non_generated](#non_generated) + 1. [protos](#protos) + 1. [src](#src) + 1. [srv](#srv) + 1. [non_generated](#non_generated-1) + 1. [templates](#templates) +1. [How to start the node](#how-to-start-the-node) +1. [Generation](#generation) + + + + + +## Content + +### build +This folder's only purpose is to exist as a temp folder during the generation. It should not be used. + +### msg +This folder contains every custom messages used by the node **kortex\_vision\_config\_driver**. All the .msg files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .msg files used by the node **kortex\_vision\_config\_driver**. + +| MSG | Description | +|:---:|:---:| +| ApiOptions.msg | A set of option that is supported by the Kortex API. It is used with the service SetApiOptions. For now there is only one option called tiemout_ms and it lets the user set a timeout value on every next service call. | +| KortexError.msg | Describe the topic /KortexError. Every service call of the node kortex_driver will publish in /KortexError everytime the Kortex API returns an error. | + + +### protos +This folder contains the protobuf files from where the MSG, SRV and sources files are generated. The content of this folder should not be modified. + +### src +This folder contains all the generated source files needed to build the node. The content of this folder should not be modified. + +### srv +This folder contains every custom services used by the node **kortex\_vision\_config\_driver**. All the .srv files in this folder are generated by a custom protoc plugin described in the [Generation](#generation) section. + +#### non_generated +This folder contains all the non-generated .srv files used by the node **kortex\_vision\_config\_driver**. + +| SRV | Description | +|:---:|:---:| +| SetApiOptions.srv | It modify the api options of the Kortex API. Once this service is called, the options set will affect every future call to the node. | +| SetDeviceID.srv | It modify the target device (device routing feature) of the node. The default value is 0.| + + +### templates +This folder contains all the JINJA2 files needed by the protoc generator. For more details on the generation process, see the [Generation](#generation) section. + +| JINJA2 files | Description | +|:---:|:---:| +| main.jinja2 | Use to generate src/main.cpp | +| NodeServices.cpp.jinja2 | Use to generate src/node.cpp | +| NodeServices.h.jinja2 | Use to generate src/node.h | +| proto_converterCPP.jinja2 | Use to generate every src/*_proto\_converter.cpp files | +| proto_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_converterCPP.jinja2 | Use to generate every src/*_ros\_converter.cpp files | +| ros_converterHeader.jinja2 | Use to generate every src/*_proto\_converter.h files | +| ros_enum.jinja2 | Use to generate every msg/*.msg files that represent a protobuf enum | +| ros_message.jinja2 | Use to generate every msg/*.msg files that represent a protobuf message | +| ros_oneof.jinja2 | Use to generate every msg/*.msg files that represent a protobuf oneof | +| ros_service.jinja2 | Use to generate every msg/*.msg files that represent a protobuf RPC | + + +## How to start the node + +rosrun kortex\_vision\_config\_driver kortex\_vision\_config\_driver 192.168.1.10 + +In the command above, you would be running the kortex\_vision\_config\_driver node on an Gen3 robot with an IP address of 192.168.1.10. + +## Generation +

+The generation process is based on a custom protobuf plugin. Basically, most of the generation process is in the python file RosGeneration.py located at the package's root folder. Before launching the generation make sure you have the python module Jinja2 installed on your computer. +

+ +To launch the generation of this package: + +1. Open a terminal window. +1. Browse the root of this package [YOUR\_ROS\_WORKSPACE]/src/ros\_kortex/kortex\_vision\_config\_driver/ +1. Make sure that the file kortex\_vision\_config\_driver.sh can be executed. If not then chmod +x kortex\_vision\_config\_driver.sh +1. Run this command: protoc --plugin=protoc-gen-custom=kortex\_vision\_config\_driver.sh -I./protos/ --custom_out=./build ./protos/\*.prot +1. The result of the generation should be on thos folder: + * /src + * /msg + * /srv + diff --git a/kortex_vision_config_driver/src/common_proto_converter.cpp b/kortex_vision_config_driver/src/common_proto_converter.cpp new file mode 100644 index 00000000..40b20611 --- /dev/null +++ b/kortex_vision_config_driver/src/common_proto_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_vision_config_driver::DeviceHandle input, DeviceHandle *output) +{ + output->set_device_type((Kinova::Api::Common::DeviceTypes)input.device_type); + output->set_device_identifier(input.device_identifier); + output->set_order(input.order); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::Empty input, Empty *output) +{ + + return 0; +} +int ToProtoData(kortex_vision_config_driver::NotificationOptions input, NotificationOptions *output) +{ + output->set_type((Kinova::Api::Common::NotificationType)input.type); + output->set_rate_m_sec(input.rate_m_sec); + output->set_threshold_value(input.threshold_value); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::SafetyHandle input, SafetyHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::NotificationHandle input, NotificationHandle *output) +{ + output->set_identifier(input.identifier); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::SafetyNotification input, SafetyNotification *output) +{ + ToProtoData(input.safety_handle, output->mutable_safety_handle()); + output->set_value((Kinova::Api::Common::SafetyStatusValue)input.value); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::Timestamp input, Timestamp *output) +{ + output->set_sec(input.sec); + output->set_usec(input.usec); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::UserProfileHandle input, UserProfileHandle *output) +{ + output->set_identifier(input.identifier); + output->set_permission(input.permission); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::Connection input, Connection *output) +{ + ToProtoData(input.user_handle, output->mutable_user_handle()); + output->set_connection_information(input.connection_information); + output->set_connection_identifier(input.connection_identifier); + + return 0; +} diff --git a/kortex_vision_config_driver/src/common_proto_converter.h b/kortex_vision_config_driver/src/common_proto_converter.h new file mode 100644 index 00000000..29706196 --- /dev/null +++ b/kortex_vision_config_driver/src/common_proto_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonPROTO_CONVERTER_H_ +#define _KORTEX_CommonPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_vision_config_driver/DeviceHandle.h" +#include "kortex_vision_config_driver/Empty.h" +#include "kortex_vision_config_driver/NotificationOptions.h" +#include "kortex_vision_config_driver/SafetyHandle.h" +#include "kortex_vision_config_driver/NotificationHandle.h" +#include "kortex_vision_config_driver/SafetyNotification.h" +#include "kortex_vision_config_driver/Timestamp.h" +#include "kortex_vision_config_driver/UserProfileHandle.h" +#include "kortex_vision_config_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToProtoData(kortex_vision_config_driver::DeviceHandle intput, DeviceHandle *output); +int ToProtoData(kortex_vision_config_driver::Empty intput, Empty *output); +int ToProtoData(kortex_vision_config_driver::NotificationOptions intput, NotificationOptions *output); +int ToProtoData(kortex_vision_config_driver::SafetyHandle intput, SafetyHandle *output); +int ToProtoData(kortex_vision_config_driver::NotificationHandle intput, NotificationHandle *output); +int ToProtoData(kortex_vision_config_driver::SafetyNotification intput, SafetyNotification *output); +int ToProtoData(kortex_vision_config_driver::Timestamp intput, Timestamp *output); +int ToProtoData(kortex_vision_config_driver::UserProfileHandle intput, UserProfileHandle *output); +int ToProtoData(kortex_vision_config_driver::Connection intput, Connection *output); + +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/src/common_ros_converter.cpp b/kortex_vision_config_driver/src/common_ros_converter.cpp new file mode 100644 index 00000000..5c3c2b0e --- /dev/null +++ b/kortex_vision_config_driver/src/common_ros_converter.cpp @@ -0,0 +1,84 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "common_ros_converter.h" + + +int ToRosData(DeviceHandle input, kortex_vision_config_driver::DeviceHandle &output) +{ + output.device_type = input.device_type(); + output.device_identifier = input.device_identifier(); + output.order = input.order(); + + return 0; +} +int ToRosData(Empty input, kortex_vision_config_driver::Empty &output) +{ + + return 0; +} +int ToRosData(NotificationOptions input, kortex_vision_config_driver::NotificationOptions &output) +{ + output.type = input.type(); + output.rate_m_sec = input.rate_m_sec(); + output.threshold_value = input.threshold_value(); + + return 0; +} +int ToRosData(SafetyHandle input, kortex_vision_config_driver::SafetyHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(NotificationHandle input, kortex_vision_config_driver::NotificationHandle &output) +{ + output.identifier = input.identifier(); + + return 0; +} +int ToRosData(SafetyNotification input, kortex_vision_config_driver::SafetyNotification &output) +{ + ToRosData(input.safety_handle(), output.safety_handle); + output.value = input.value(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + return 0; +} +int ToRosData(Timestamp input, kortex_vision_config_driver::Timestamp &output) +{ + output.sec = input.sec(); + output.usec = input.usec(); + + return 0; +} +int ToRosData(UserProfileHandle input, kortex_vision_config_driver::UserProfileHandle &output) +{ + output.identifier = input.identifier(); + output.permission = input.permission(); + + return 0; +} +int ToRosData(Connection input, kortex_vision_config_driver::Connection &output) +{ + ToRosData(input.user_handle(), output.user_handle); + output.connection_information = input.connection_information(); + output.connection_identifier = input.connection_identifier(); + + return 0; +} diff --git a/kortex_vision_config_driver/src/common_ros_converter.h b/kortex_vision_config_driver/src/common_ros_converter.h new file mode 100644 index 00000000..59335369 --- /dev/null +++ b/kortex_vision_config_driver/src/common_ros_converter.h @@ -0,0 +1,66 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_CommonROS_CONVERTER_H_ +#define _KORTEX_CommonROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_vision_config_driver/DeviceHandle.h" +#include "kortex_vision_config_driver/Empty.h" +#include "kortex_vision_config_driver/NotificationOptions.h" +#include "kortex_vision_config_driver/SafetyHandle.h" +#include "kortex_vision_config_driver/NotificationHandle.h" +#include "kortex_vision_config_driver/SafetyNotification.h" +#include "kortex_vision_config_driver/Timestamp.h" +#include "kortex_vision_config_driver/UserProfileHandle.h" +#include "kortex_vision_config_driver/Connection.h" + + +using namespace Kinova::Api::Common; + +int ToRosData(DeviceHandle input, kortex_vision_config_driver::DeviceHandle &output); +int ToRosData(Empty input, kortex_vision_config_driver::Empty &output); +int ToRosData(NotificationOptions input, kortex_vision_config_driver::NotificationOptions &output); +int ToRosData(SafetyHandle input, kortex_vision_config_driver::SafetyHandle &output); +int ToRosData(NotificationHandle input, kortex_vision_config_driver::NotificationHandle &output); +int ToRosData(SafetyNotification input, kortex_vision_config_driver::SafetyNotification &output); +int ToRosData(Timestamp input, kortex_vision_config_driver::Timestamp &output); +int ToRosData(UserProfileHandle input, kortex_vision_config_driver::UserProfileHandle &output); +int ToRosData(Connection input, kortex_vision_config_driver::Connection &output); + +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/src/main.cpp b/kortex_vision_config_driver/src/main.cpp new file mode 100644 index 00000000..6a4d3311 --- /dev/null +++ b/kortex_vision_config_driver/src/main.cpp @@ -0,0 +1,75 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "VisionConfig_Services"); + + uint32_t device_id = 0; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 2) + { + stringstream tempId; + tempId << argv[2]; + tempId >> device_id; + + if(tempId.fail() || tempId.bad()) + { + ROS_INFO("ERROR - Bad device ID, shutting down the node..."); + ros::shutdown(); + return 0; + } + ROS_INFO("Connecting to IP = %s - device ID = %s", argv[1], argv[2]); + } + else if(argc > 1) + { + ROS_INFO("Connecting to IP = %s", argv[1]); + } + else + { + ROS_INFO("You need to provide, at least, an IP adresse as the first parameter. An optional device ID can also be passed if you are using the device routing feature. ex: rosrun package node 192.168.1.10 [device_id]"); + ros::shutdown(); + return 0; + } + + VisionConfig_Services services_object(argv[1], n, device_id); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &VisionConfig_Services::SetDeviceID, &services_object); + ros::ServiceServer serviceSetApiOptions = n.advertiseService("SetApiOptions", &VisionConfig_Services::SetApiOptions, &services_object); + + ros::ServiceServer serviceSetSensorSettings = n.advertiseService("SetSensorSettings", &VisionConfig_Services::SetSensorSettings, &services_object); + ros::ServiceServer serviceGetSensorSettings = n.advertiseService("GetSensorSettings", &VisionConfig_Services::GetSensorSettings, &services_object); + ros::ServiceServer serviceGetOptionValue = n.advertiseService("GetOptionValue", &VisionConfig_Services::GetOptionValue, &services_object); + ros::ServiceServer serviceSetOptionValue = n.advertiseService("SetOptionValue", &VisionConfig_Services::SetOptionValue, &services_object); + ros::ServiceServer serviceGetOptionInformation = n.advertiseService("GetOptionInformation", &VisionConfig_Services::GetOptionInformation, &services_object); + ros::ServiceServer serviceDoSensorFocusAction = n.advertiseService("DoSensorFocusAction", &VisionConfig_Services::DoSensorFocusAction, &services_object); + ros::ServiceServer serviceGetIntrinsicParameters = n.advertiseService("GetIntrinsicParameters", &VisionConfig_Services::GetIntrinsicParameters, &services_object); + + + ROS_INFO("Node's services initialized correctly."); + + ros::spin(); + + return 1; +} \ No newline at end of file diff --git a/kortex_vision_config_driver/src/node.cpp b/kortex_vision_config_driver/src/node.cpp new file mode 100644 index 00000000..17a3735e --- /dev/null +++ b/kortex_vision_config_driver/src/node.cpp @@ -0,0 +1,300 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +#include "common_ros_converter.h" +#include "common_proto_converter.h" +#include "visionconfig_ros_converter.h" +#include "visionconfig_proto_converter.h" +VisionConfig_Services::VisionConfig_Services(char* ip, ros::NodeHandle& n, uint32_t device_id) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = device_id; + m_apiOptions.timeout_ms = 3000; + + m_visionconfig = new VisionConfig::VisionConfigClient(m_router);//If the Device ID is different than 0, it means that we are using the feature DEVICE ROUTING. + if(m_CurrentDeviceID != 0) + { + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + } + + m_pub_Error = m_n.advertise("KortexError", 1000); + m_pub_VisionTopic = m_n.advertise("VisionTopic", 1000);std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool VisionConfig_Services::SetDeviceID(kortex_vision_config_driver::SetDeviceID::Request &req, kortex_vision_config_driver::SetDeviceID::Response &res) +{ + if(m_CurrentDeviceID == 0) + { + auto sessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + sessionManager->CreateSession(createSessionInfo); + } + + m_CurrentDeviceID = req.device_id; +} + +bool VisionConfig_Services::SetApiOptions(kortex_vision_config_driver::SetApiOptions::Request &req, kortex_vision_config_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} + + + +bool VisionConfig_Services::SetSensorSettings(kortex_vision_config_driver::SetSensorSettings::Request &req, kortex_vision_config_driver::SetSensorSettings::Response &res) +{ + SensorSettings input; + ToProtoData(req.input, &input); + Empty output; + kortex_vision_config_driver::KortexError result_error; + + try + { + m_visionconfig->SetSensorSettings(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool VisionConfig_Services::GetSensorSettings(kortex_vision_config_driver::GetSensorSettings::Request &req, kortex_vision_config_driver::GetSensorSettings::Response &res) +{ + SensorIdentifier input; + ToProtoData(req.input, &input); + SensorSettings output; + kortex_vision_config_driver::KortexError result_error; + + try + { + output = m_visionconfig->GetSensorSettings(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool VisionConfig_Services::GetOptionValue(kortex_vision_config_driver::GetOptionValue::Request &req, kortex_vision_config_driver::GetOptionValue::Response &res) +{ + OptionIdentifier input; + ToProtoData(req.input, &input); + OptionValue output; + kortex_vision_config_driver::KortexError result_error; + + try + { + output = m_visionconfig->GetOptionValue(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool VisionConfig_Services::SetOptionValue(kortex_vision_config_driver::SetOptionValue::Request &req, kortex_vision_config_driver::SetOptionValue::Response &res) +{ + OptionValue input; + ToProtoData(req.input, &input); + Empty output; + kortex_vision_config_driver::KortexError result_error; + + try + { + m_visionconfig->SetOptionValue(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool VisionConfig_Services::GetOptionInformation(kortex_vision_config_driver::GetOptionInformation::Request &req, kortex_vision_config_driver::GetOptionInformation::Response &res) +{ + OptionIdentifier input; + ToProtoData(req.input, &input); + OptionInformation output; + kortex_vision_config_driver::KortexError result_error; + + try + { + output = m_visionconfig->GetOptionInformation(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} + +bool VisionConfig_Services::OnNotificationVisionTopic(kortex_vision_config_driver::VisionTopic::Request &req, kortex_vision_config_driver::VisionTopic::Response &res) +{ + NotificationOptions input; + ToProtoData(req.input, &input); + NotificationHandle output; + kortex_vision_config_driver::KortexError result_error; + + try + { + std::function< void (VisionConfig::VisionNotification) > callback = std::bind(&VisionConfig_Services::cb_VisionTopic, this, std::placeholders::_1); + output = m_visionconfig->OnNotificationVisionTopic(callback, input); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} +void VisionConfig_Services::cb_VisionTopic(VisionConfig::VisionNotification notif) +{ + kortex_vision_config_driver::VisionNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_VisionTopic.publish(ros_msg); +} + +bool VisionConfig_Services::DoSensorFocusAction(kortex_vision_config_driver::DoSensorFocusAction::Request &req, kortex_vision_config_driver::DoSensorFocusAction::Response &res) +{ + SensorFocusAction input; + ToProtoData(req.input, &input); + Empty output; + kortex_vision_config_driver::KortexError result_error; + + try + { + m_visionconfig->DoSensorFocusAction(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + return true; +} + +bool VisionConfig_Services::GetIntrinsicParameters(kortex_vision_config_driver::GetIntrinsicParameters::Request &req, kortex_vision_config_driver::GetIntrinsicParameters::Response &res) +{ + SensorIdentifier input; + ToProtoData(req.input, &input); + IntrinsicParameters output; + kortex_vision_config_driver::KortexError result_error; + + try + { + output = m_visionconfig->GetIntrinsicParameters(input, m_CurrentDeviceID, m_apiOptions); + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_vision_config_driver/src/node.h b/kortex_vision_config_driver/src/node.h new file mode 100644 index 00000000..b5260fc6 --- /dev/null +++ b/kortex_vision_config_driver/src/node.h @@ -0,0 +1,93 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_SERVICES_H_ +#define _KORTEX_SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include "kortex_vision_config_driver/SetSensorSettings.h" +#include "kortex_vision_config_driver/GetSensorSettings.h" +#include "kortex_vision_config_driver/GetOptionValue.h" +#include "kortex_vision_config_driver/SetOptionValue.h" +#include "kortex_vision_config_driver/GetOptionInformation.h" +#include "kortex_vision_config_driver/VisionTopic.h" +#include "kortex_vision_config_driver/DoSensorFocusAction.h" +#include "kortex_vision_config_driver/GetIntrinsicParameters.h" +#include "kortex_vision_config_driver/KortexError.h" +#include "kortex_vision_config_driver/SetDeviceID.h" +#include "kortex_vision_config_driver/SetApiOptions.h" + +#include "kortex_vision_config_driver/ApiOptions.h" + +using namespace std; +using namespace Kinova::Api; +using namespace Kinova::Api::Common; +using namespace Kinova::Api::VisionConfig; + +class VisionConfig_Services +{ + public: + VisionConfig_Services(char* ip, ros::NodeHandle& n, uint32_t device_id); + bool SetDeviceID(kortex_vision_config_driver::SetDeviceID::Request &req, kortex_vision_config_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_vision_config_driver::SetApiOptions::Request &req, kortex_vision_config_driver::SetApiOptions::Response &res); + + + bool SetSensorSettings(kortex_vision_config_driver::SetSensorSettings::Request &req, kortex_vision_config_driver::SetSensorSettings::Response &res); + bool GetSensorSettings(kortex_vision_config_driver::GetSensorSettings::Request &req, kortex_vision_config_driver::GetSensorSettings::Response &res); + bool GetOptionValue(kortex_vision_config_driver::GetOptionValue::Request &req, kortex_vision_config_driver::GetOptionValue::Response &res); + bool SetOptionValue(kortex_vision_config_driver::SetOptionValue::Request &req, kortex_vision_config_driver::SetOptionValue::Response &res); + bool GetOptionInformation(kortex_vision_config_driver::GetOptionInformation::Request &req, kortex_vision_config_driver::GetOptionInformation::Response &res); + bool OnNotificationVisionTopic(kortex_vision_config_driver::VisionTopic::Request &req, kortex_vision_config_driver::VisionTopic::Response &res); + void cb_VisionTopic(VisionNotification notif); + bool DoSensorFocusAction(kortex_vision_config_driver::DoSensorFocusAction::Request &req, kortex_vision_config_driver::DoSensorFocusAction::Response &res); + bool GetIntrinsicParameters(kortex_vision_config_driver::GetIntrinsicParameters::Request &req, kortex_vision_config_driver::GetIntrinsicParameters::Response &res); + + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + + VisionConfigClient* m_visionconfig; + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + ros::Publisher m_pub_VisionTopic; +}; +#endif diff --git a/kortex_vision_config_driver/src/util/diagnostic.h b/kortex_vision_config_driver/src/util/diagnostic.h new file mode 100644 index 00000000..f0199870 --- /dev/null +++ b/kortex_vision_config_driver/src/util/diagnostic.h @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +struct kortex_error +{ + int error_code; + std::string description; +}; \ No newline at end of file diff --git a/kortex_vision_config_driver/src/util/math_util.h b/kortex_vision_config_driver/src/util/math_util.h new file mode 100644 index 00000000..bf935386 --- /dev/null +++ b/kortex_vision_config_driver/src/util/math_util.h @@ -0,0 +1,12 @@ +/* + * Copyright (c) 2018 Kinova inc. All rights reserved. + * + * This software may be modified and distributed under the + * terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ +#include + +#define TO_RAD(degree) degree * M_PI / 180.0 \ No newline at end of file diff --git a/kortex_vision_config_driver/src/visionconfig_proto_converter.cpp b/kortex_vision_config_driver/src/visionconfig_proto_converter.cpp new file mode 100644 index 00000000..dbec65ae --- /dev/null +++ b/kortex_vision_config_driver/src/visionconfig_proto_converter.cpp @@ -0,0 +1,90 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "visionconfig_proto_converter.h" + +#include "common_proto_converter.h" + + +int ToProtoData(kortex_vision_config_driver::SensorSettings input, SensorSettings *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_resolution((Kinova::Api::VisionConfig::Resolution)input.resolution); + output->set_frame_rate((Kinova::Api::VisionConfig::FrameRate)input.frame_rate); + output->set_bit_rate((Kinova::Api::VisionConfig::BitRate)input.bit_rate); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::SensorIdentifier input, SensorIdentifier *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::OptionIdentifier input, OptionIdentifier *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_option((Kinova::Api::VisionConfig::Option)input.option); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::OptionValue input, OptionValue *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_option((Kinova::Api::VisionConfig::Option)input.option); + output->set_value(input.value); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::OptionInformation input, OptionInformation *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_option((Kinova::Api::VisionConfig::Option)input.option); + output->set_supported(input.supported); + output->set_read_only(input.read_only); + output->set_minimum(input.minimum); + output->set_maximum(input.maximum); + output->set_step(input.step); + output->set_default_value(input.default_value); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::SensorFocusAction input, SensorFocusAction *output) +{ + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_focus_action((Kinova::Api::VisionConfig::FocusAction)input.focus_action); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::VisionNotification input, VisionNotification *output) +{ + output->set_event((Kinova::Api::VisionConfig::VisionEvent)input.event); + output->set_sensor((Kinova::Api::VisionConfig::Sensor)input.sensor); + output->set_option((Kinova::Api::VisionConfig::Option)input.option); + + return 0; +} +int ToProtoData(kortex_vision_config_driver::IntrinsicParameters input, IntrinsicParameters *output) +{ + output->set_width(input.width); + output->set_height(input.height); + output->set_principal_point_x(input.principal_point_x); + output->set_principal_point_y(input.principal_point_y); + output->set_focal_length_x(input.focal_length_x); + output->set_focal_length_y(input.focal_length_y); + + return 0; +} diff --git a/kortex_vision_config_driver/src/visionconfig_proto_converter.h b/kortex_vision_config_driver/src/visionconfig_proto_converter.h new file mode 100644 index 00000000..dc4e3e7f --- /dev/null +++ b/kortex_vision_config_driver/src/visionconfig_proto_converter.h @@ -0,0 +1,64 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VisionConfigPROTO_CONVERTER_H_ +#define _KORTEX_VisionConfigPROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_vision_config_driver/SensorSettings.h" +#include "kortex_vision_config_driver/SensorIdentifier.h" +#include "kortex_vision_config_driver/OptionIdentifier.h" +#include "kortex_vision_config_driver/OptionValue.h" +#include "kortex_vision_config_driver/OptionInformation.h" +#include "kortex_vision_config_driver/SensorFocusAction.h" +#include "kortex_vision_config_driver/VisionNotification.h" +#include "kortex_vision_config_driver/IntrinsicParameters.h" + + +using namespace Kinova::Api::VisionConfig; + +int ToProtoData(kortex_vision_config_driver::SensorSettings intput, SensorSettings *output); +int ToProtoData(kortex_vision_config_driver::SensorIdentifier intput, SensorIdentifier *output); +int ToProtoData(kortex_vision_config_driver::OptionIdentifier intput, OptionIdentifier *output); +int ToProtoData(kortex_vision_config_driver::OptionValue intput, OptionValue *output); +int ToProtoData(kortex_vision_config_driver::OptionInformation intput, OptionInformation *output); +int ToProtoData(kortex_vision_config_driver::SensorFocusAction intput, SensorFocusAction *output); +int ToProtoData(kortex_vision_config_driver::VisionNotification intput, VisionNotification *output); +int ToProtoData(kortex_vision_config_driver::IntrinsicParameters intput, IntrinsicParameters *output); + +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/src/visionconfig_ros_converter.cpp b/kortex_vision_config_driver/src/visionconfig_ros_converter.cpp new file mode 100644 index 00000000..9a1efe16 --- /dev/null +++ b/kortex_vision_config_driver/src/visionconfig_ros_converter.cpp @@ -0,0 +1,90 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "visionconfig_ros_converter.h" + +#include "common_ros_converter.h" + + +int ToRosData(SensorSettings input, kortex_vision_config_driver::SensorSettings &output) +{ + output.sensor = input.sensor(); + output.resolution = input.resolution(); + output.frame_rate = input.frame_rate(); + output.bit_rate = input.bit_rate(); + + return 0; +} +int ToRosData(SensorIdentifier input, kortex_vision_config_driver::SensorIdentifier &output) +{ + output.sensor = input.sensor(); + + return 0; +} +int ToRosData(OptionIdentifier input, kortex_vision_config_driver::OptionIdentifier &output) +{ + output.sensor = input.sensor(); + output.option = input.option(); + + return 0; +} +int ToRosData(OptionValue input, kortex_vision_config_driver::OptionValue &output) +{ + output.sensor = input.sensor(); + output.option = input.option(); + output.value = input.value(); + + return 0; +} +int ToRosData(OptionInformation input, kortex_vision_config_driver::OptionInformation &output) +{ + output.sensor = input.sensor(); + output.option = input.option(); + output.supported = input.supported(); + output.read_only = input.read_only(); + output.minimum = input.minimum(); + output.maximum = input.maximum(); + output.step = input.step(); + output.default_value = input.default_value(); + + return 0; +} +int ToRosData(SensorFocusAction input, kortex_vision_config_driver::SensorFocusAction &output) +{ + output.sensor = input.sensor(); + output.focus_action = input.focus_action(); + + return 0; +} +int ToRosData(VisionNotification input, kortex_vision_config_driver::VisionNotification &output) +{ + output.event = input.event(); + output.sensor = input.sensor(); + output.option = input.option(); + + return 0; +} +int ToRosData(IntrinsicParameters input, kortex_vision_config_driver::IntrinsicParameters &output) +{ + output.width = input.width(); + output.height = input.height(); + output.principal_point_x = input.principal_point_x(); + output.principal_point_y = input.principal_point_y(); + output.focal_length_x = input.focal_length_x(); + output.focal_length_y = input.focal_length_y(); + + return 0; +} diff --git a/kortex_vision_config_driver/src/visionconfig_ros_converter.h b/kortex_vision_config_driver/src/visionconfig_ros_converter.h new file mode 100644 index 00000000..dd1c9f4c --- /dev/null +++ b/kortex_vision_config_driver/src/visionconfig_ros_converter.h @@ -0,0 +1,64 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_VisionConfigROS_CONVERTER_H_ +#define _KORTEX_VisionConfigROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "kortex_vision_config_driver/SensorSettings.h" +#include "kortex_vision_config_driver/SensorIdentifier.h" +#include "kortex_vision_config_driver/OptionIdentifier.h" +#include "kortex_vision_config_driver/OptionValue.h" +#include "kortex_vision_config_driver/OptionInformation.h" +#include "kortex_vision_config_driver/SensorFocusAction.h" +#include "kortex_vision_config_driver/VisionNotification.h" +#include "kortex_vision_config_driver/IntrinsicParameters.h" + + +using namespace Kinova::Api::VisionConfig; + +int ToRosData(SensorSettings input, kortex_vision_config_driver::SensorSettings &output); +int ToRosData(SensorIdentifier input, kortex_vision_config_driver::SensorIdentifier &output); +int ToRosData(OptionIdentifier input, kortex_vision_config_driver::OptionIdentifier &output); +int ToRosData(OptionValue input, kortex_vision_config_driver::OptionValue &output); +int ToRosData(OptionInformation input, kortex_vision_config_driver::OptionInformation &output); +int ToRosData(SensorFocusAction input, kortex_vision_config_driver::SensorFocusAction &output); +int ToRosData(VisionNotification input, kortex_vision_config_driver::VisionNotification &output); +int ToRosData(IntrinsicParameters input, kortex_vision_config_driver::IntrinsicParameters &output); + +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/DoSensorFocusAction.srv b/kortex_vision_config_driver/srv/DoSensorFocusAction.srv new file mode 100644 index 00000000..96d7baa8 --- /dev/null +++ b/kortex_vision_config_driver/srv/DoSensorFocusAction.srv @@ -0,0 +1,3 @@ +SensorFocusAction input +--- +Empty output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/GetIntrinsicParameters.srv b/kortex_vision_config_driver/srv/GetIntrinsicParameters.srv new file mode 100644 index 00000000..706f2ab9 --- /dev/null +++ b/kortex_vision_config_driver/srv/GetIntrinsicParameters.srv @@ -0,0 +1,3 @@ +SensorIdentifier input +--- +IntrinsicParameters output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/GetOptionInformation.srv b/kortex_vision_config_driver/srv/GetOptionInformation.srv new file mode 100644 index 00000000..995b3a8e --- /dev/null +++ b/kortex_vision_config_driver/srv/GetOptionInformation.srv @@ -0,0 +1,3 @@ +OptionIdentifier input +--- +OptionInformation output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/GetOptionValue.srv b/kortex_vision_config_driver/srv/GetOptionValue.srv new file mode 100644 index 00000000..be416735 --- /dev/null +++ b/kortex_vision_config_driver/srv/GetOptionValue.srv @@ -0,0 +1,3 @@ +OptionIdentifier input +--- +OptionValue output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/GetSensorSettings.srv b/kortex_vision_config_driver/srv/GetSensorSettings.srv new file mode 100644 index 00000000..c6636980 --- /dev/null +++ b/kortex_vision_config_driver/srv/GetSensorSettings.srv @@ -0,0 +1,3 @@ +SensorIdentifier input +--- +SensorSettings output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/SetOptionValue.srv b/kortex_vision_config_driver/srv/SetOptionValue.srv new file mode 100644 index 00000000..3214d55b --- /dev/null +++ b/kortex_vision_config_driver/srv/SetOptionValue.srv @@ -0,0 +1,3 @@ +OptionValue input +--- +Empty output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/SetSensorSettings.srv b/kortex_vision_config_driver/srv/SetSensorSettings.srv new file mode 100644 index 00000000..e5e58687 --- /dev/null +++ b/kortex_vision_config_driver/srv/SetSensorSettings.srv @@ -0,0 +1,3 @@ +SensorSettings input +--- +Empty output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/VisionTopic.srv b/kortex_vision_config_driver/srv/VisionTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_vision_config_driver/srv/VisionTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_vision_config_driver/srv/non_generated/SetApiOptions.srv b/kortex_vision_config_driver/srv/non_generated/SetApiOptions.srv new file mode 100644 index 00000000..cab7c810 --- /dev/null +++ b/kortex_vision_config_driver/srv/non_generated/SetApiOptions.srv @@ -0,0 +1,3 @@ +ApiOptions input +--- + diff --git a/kortex_vision_config_driver/srv/non_generated/SetDeviceID.srv b/kortex_vision_config_driver/srv/non_generated/SetDeviceID.srv new file mode 100644 index 00000000..396957c5 --- /dev/null +++ b/kortex_vision_config_driver/srv/non_generated/SetDeviceID.srv @@ -0,0 +1,2 @@ +uint32 device_id +--- diff --git a/kortex_vision_config_driver/templates/NodeServices.cpp.jinja2 b/kortex_vision_config_driver/templates/NodeServices.cpp.jinja2 new file mode 100644 index 00000000..8bb883ad --- /dev/null +++ b/kortex_vision_config_driver/templates/NodeServices.cpp.jinja2 @@ -0,0 +1,151 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" +{% for package in detailedPackages %} +{%- if package.HasMessage == 1 -%} +#include "{{package.filename|lower}}_ros_converter.h" +#include "{{package.filename|lower}}_proto_converter.h" +{%- endif %} +{% endfor -%} + +VisionConfig_Services::VisionConfig_Services(char* ip, ros::NodeHandle& n, uint32_t device_id) : m_n(n) +{ + m_transport = new TransportClientUdp(); + m_transport->connect(ip, 10000); + + m_router = new RouterClient(m_transport, [](KError err) { cout << "_________ callback error _________" << err.toString(); }); + m_CurrentDeviceID = device_id; + m_apiOptions.timeout_ms = 3000; +{% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + m_{{package.name|lower}} = new {{package.name}}::{{package.name}}Client(m_router); + {%- endif -%} +{% endfor -%} + //If the Device ID is different than 0, it means that we are using the feature DEVICE ROUTING. + if(m_CurrentDeviceID != 0) + { + m_SessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + m_SessionManager->CreateSession(createSessionInfo); + } + + m_pub_Error = m_n.advertise("KortexError", 1000); +{%- for package in detailedPackages -%} +{%- for method in package.service.method -%} +{%- if 'Topic' in method.name %} + m_pub_{{method.name}} = m_n.advertise("{{method.name}}", 1000); +{%- endif -%} +{%- endfor -%} +{%- endfor -%} + + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +bool VisionConfig_Services::SetDeviceID(kortex_vision_config_driver::SetDeviceID::Request &req, kortex_vision_config_driver::SetDeviceID::Response &res) +{ + if(m_CurrentDeviceID == 0) + { + auto sessionManager = new SessionManager(m_router); + auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo(); + + createSessionInfo.set_username("admin"); + createSessionInfo.set_password("admin"); + createSessionInfo.set_session_inactivity_timeout(35000); + + sessionManager->CreateSession(createSessionInfo); + } + + m_CurrentDeviceID = req.device_id; +} + +bool VisionConfig_Services::SetApiOptions(kortex_vision_config_driver::SetApiOptions::Request &req, kortex_vision_config_driver::SetApiOptions::Response &res) +{ + m_apiOptions.timeout_ms = req.input.timeout_ms; + + return true; +} +{% for package in detailedPackages %} +{% for method in package.service.method %} +{%- if 'Topic' in method.name %} +bool VisionConfig_Services::OnNotification{{method.name}}(kortex_vision_config_driver::{{method.name}}::Request &req, kortex_vision_config_driver::{{method.name}}::Response &res) +{%- else %} +bool VisionConfig_Services::{{method.name}}(kortex_vision_config_driver::{{method.name}}::Request &req, kortex_vision_config_driver::{{method.name}}::Response &res) +{%- endif %} +{ + {%- set splitInputTypeName = method.input_type.split('.') -%} + {% set splitOutputTypeName = method.output_type.split('.') %} + {{splitInputTypeName[4]}} input; + {%- if not method.input_type.split('.')[4] == "Empty" %} + ToProtoData(req.input, &input); + {%- endif %} + {{splitOutputTypeName[4]}} output; + kortex_vision_config_driver::KortexError result_error; + + try + { + {%- if not method.output_type.split('.')[4] == "Empty" %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + {%- if 'Topic' in method.name %} + std::function< void ({{package.name}}::{{method.name|replace("Topic", "")}}Notification) > callback = std::bind(&VisionConfig_Services::cb_{{method.name}}, this, std::placeholders::_1); + output = m_{{package.name|lower}}->OnNotification{{method.name}}(callback, input); + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + output = m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- else %} + {%- if not method.input_type.split('.')[4] == "Empty" %} + m_{{package.name|lower}}->{{method.name}}(input, m_CurrentDeviceID, m_apiOptions); + {%- else %} + m_{{package.name|lower}}->{{method.name}}(m_CurrentDeviceID, m_apiOptions); + {%- endif %} + {%- endif %} + } + catch (KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.what(); + m_pub_Error.publish(result_error); + ROS_INFO("KINOVA exception: %d\n", ex.getErrorInfo().getError().error_sub_code()); + return false; + } + catch (std::runtime_error& ex2) + { + return false; + } + {%- if not method.output_type.split('.')[4] == "Empty" %} + ToRosData(output, res.output); + {%- endif %} + return true; +} +{%- if 'Topic' in method.name %} +void VisionConfig_Services::cb_{{method.name}}({{package.name}}::{{method.name|replace("Topic", "")}}Notification notif) +{ + kortex_vision_config_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + ToRosData(notif, ros_msg); + m_pub_{{method.name}}.publish(ros_msg); +} +{%- endif %} +{% endfor -%} +{% endfor -%} \ No newline at end of file diff --git a/kortex_vision_config_driver/templates/NodeServices.h.jinja2 b/kortex_vision_config_driver/templates/NodeServices.h.jinja2 new file mode 100644 index 00000000..e7999ee1 --- /dev/null +++ b/kortex_vision_config_driver/templates/NodeServices.h.jinja2 @@ -0,0 +1,107 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{packageName}}SERVICES_H_ +#define _KORTEX_{{packageName}}SERVICES_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +{%- for package in detailedPackages %} +#include <{{package.filename}}.pb.h> +{%- endfor %} + +#include +#include + +#include +#include + +{%- for package in detailedPackages %} +{%- if package.HasRPC == 1 %} +#include <{{package.name}}ClientRpc.h> +{%- endif %} +{%- endfor %} +#include +#include + +{%- for package in detailedPackages %} +{%- for method in package.service.method %} +#include "kortex_vision_config_driver/{{method.name}}.h" +{%- endfor %} +{%- endfor %} +#include "kortex_vision_config_driver/KortexError.h" +#include "kortex_vision_config_driver/SetDeviceID.h" +#include "kortex_vision_config_driver/SetApiOptions.h" + +#include "kortex_vision_config_driver/ApiOptions.h" + +using namespace std; +using namespace Kinova::Api; +{%- for package in detailedPackages %} +using namespace {{package.namespace}}; +{%- endfor %} + +class VisionConfig_Services +{ + public: + VisionConfig_Services(char* ip, ros::NodeHandle& n, uint32_t device_id); + bool SetDeviceID(kortex_vision_config_driver::SetDeviceID::Request &req, kortex_vision_config_driver::SetDeviceID::Response &res); + bool SetApiOptions(kortex_vision_config_driver::SetApiOptions::Request &req, kortex_vision_config_driver::SetApiOptions::Response &res); +{% for package in detailedPackages %} +{%- for method in package.service.method %} +{%- if 'Topic' in method.name %} + bool OnNotification{{method.name}}(kortex_vision_config_driver::{{method.name}}::Request &req, kortex_vision_config_driver::{{method.name}}::Response &res); + void cb_{{method.name}}({{method.name|replace("Topic", "")}}Notification notif); +{%- else %} + bool {{method.name}}(kortex_vision_config_driver::{{method.name}}::Request &req, kortex_vision_config_driver::{{method.name}}::Response &res); +{%- endif %} +{%- endfor %} +{% endfor %} + +private: + TransportClientUdp* m_transport; + RouterClient* m_router; + {% for package in detailedPackages %} + {%- if package.HasRPC == 1 %} + {{package.name}}Client* m_{{package.name|lower}}; + {%- endif -%} + {% endfor %} + uint32_t m_CurrentDeviceID; + RouterClientSendOptions m_apiOptions; + + SessionManager* m_SessionManager; + + ros::NodeHandle m_n; + ros::Publisher m_pub_Error; + + {%- for package in detailedPackages %} + {%- for method in package.service.method %} + {%- if 'Topic' in method.name %} + ros::Publisher m_pub_{{method.name}}; + {%- endif %} + {%- endfor %} + {%- endfor %} +}; +#endif + diff --git a/kortex_vision_config_driver/templates/main.jinja2 b/kortex_vision_config_driver/templates/main.jinja2 new file mode 100644 index 00000000..93994931 --- /dev/null +++ b/kortex_vision_config_driver/templates/main.jinja2 @@ -0,0 +1,70 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "node.h" + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "VisionConfig_Services"); + + uint32_t device_id = 0; + + ros::NodeHandle n; + bool valid_ip = false; + + if(argc > 2) + { + stringstream tempId; + tempId << argv[2]; + tempId >> device_id; + + if(tempId.fail() || tempId.bad()) + { + ROS_INFO("ERROR - Bad device ID, shutting down the node..."); + ros::shutdown(); + return 0; + } + ROS_INFO("Connecting to IP = %s - device ID = %s", argv[1], argv[2]); + } + else if(argc > 1) + { + ROS_INFO("Connecting to IP = %s", argv[1]); + } + else + { + ROS_INFO("You need to provide, at least, an IP adresse as the first parameter. An optional device ID can also be passed if you are using the device routing feature. ex: rosrun package node 192.168.1.10 [device_id]"); + ros::shutdown(); + return 0; + } + + VisionConfig_Services services_object(argv[1], n, device_id); + + ros::ServiceServer serviceSetDeviceID = n.advertiseService("SetDeviceID", &VisionConfig_Services::SetDeviceID, &services_object); + ros::ServiceServer serviceSetApiOptions = n.advertiseService("SetApiOptions", &VisionConfig_Services::SetApiOptions, &services_object); + + {% for function in list_function -%} + ros::ServiceServer service{{function}} = n.advertiseService("{{function}}", &VisionConfig_Services::{{function}}, &services_object); + {% endfor %} + + ROS_INFO("Node's services initialized correctly."); + + ros::spin(); + + return 1; +} diff --git a/kortex_vision_config_driver/templates/proto_converter.cpp.jinja2 b/kortex_vision_config_driver/templates/proto_converter.cpp.jinja2 new file mode 100644 index 00000000..5f255569 --- /dev/null +++ b/kortex_vision_config_driver/templates/proto_converter.cpp.jinja2 @@ -0,0 +1,77 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentFilename|lower}}_proto_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_proto_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToProtoData(kortex_vision_config_driver::{{detailed_message.message.name}} input, {{detailed_message.message.name}} *output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") -%} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {%- if field.type == 11 %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + ToProtoData(input.{{field.name}}[i], output->add_{{field.name|lower}}()); + } + {%- else %} + output->clear_{{field.name|lower}}(); + for(int i = 0; i < input.{{field.name}}.size(); i++) + { + output->add_{{field.name|lower}}(input.{{field.name}}[i]); + } + {% endif -%} + {% else -%} + {%- if field.type == 11 %} + ToProtoData(input.{{field.name}}, output->mutable_{{field.name}}()); + {%- elif field.type == 14 %}{# ENUM #} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output->set_{{field.name}}(({{field.type_name|replace(".", "", 1)|replace(".", "::")}})input.{{field.name}}); + {%- elif field.type == 12 %} + output->set_{{field.name}}(std::string(input.{{field.name}}.begin(), input.{{field.name}}.end())); + {%- else %} + output->set_{{field.name}}(input.{{field.name}}); + {%- endif -%} + {%- endif -%} + {%- endif -%} + {%- endfor -%} + + {% if detailed_message.HasOneOf == "true" %} + + {% for field in detailed_message.message.field %} + {%- if field.HasField("oneof_index") -%} + if(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.size() > 0) + { + {% if field.type == 11 -%} + ToProtoData(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0], output->mutable_{{field.name}}()); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} + output->set_{{field.name}}(({{list1[4]}})input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- else %} + output->set_{{field.name}}(input.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}[0]); + {%- endif %} + } + {% endif %} + {%- endfor -%} + {% endif %} + + return 0; +} +{% endfor %} diff --git a/kortex_vision_config_driver/templates/proto_converter.h.jinja2 b/kortex_vision_config_driver/templates/proto_converter.h.jinja2 new file mode 100644 index 00000000..3f56d39f --- /dev/null +++ b/kortex_vision_config_driver/templates/proto_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}PROTO_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_vision_config_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToProtoData(kortex_vision_config_driver::{{detailed_message.message.name}} intput, {{detailed_message.message.name}} *output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/templates/ros_converter.cpp.jinja2 b/kortex_vision_config_driver/templates/ros_converter.cpp.jinja2 new file mode 100644 index 00000000..8ddcc9e7 --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_converter.cpp.jinja2 @@ -0,0 +1,86 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#include "{{currentFilename|lower}}_ros_converter.h" +{% if 'Common' not in currentPackageName %} +#include "common_ros_converter.h" +{% endif %} + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_vision_config_driver::{{detailed_message.message.name}} &output) +{ + {%- for field in detailed_message.message.field %} + {%- if not field.HasField("oneof_index") %} + {%- if field.label == 3 %} {# Si c'est un repeated #} + {% if field.type == 11 %} + {%- set splitTypeName = field.type_name.split('.') -%} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + gen3_actuator::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(i), temp); + output.{{field.name}}.push_back(temp); + } + {%- else %} + output.{{field.name}}.clear(); + for(int i = 0; i < input.{{field.name|lower}}_size(); i++) + { + output.{{field.name}}.push_back(input.{{field.name|lower}}(i)); + } + {%- endif %} + {%- else %} + {%- if field.type == 11 %} + ToRosData(input.{{field.name}}(), output.{{field.name}}); + {%- elif field.type == 14 %} + {%- set list1 = field.type_name.split('.') -%} {# Cette ligne sert à enlever les namespace dans le nom du type #} + output.{{field.name}} = input.{{field.name}}(); + {%- elif field.type == 12 %} + output.{{field.name}} = std::vector(input.{{field.name}}().begin(), input.{{field.name}}().end()); + {%- else %} + output.{{field.name}} = input.{{field.name}}(); + {%- endif %} + {%- endif %} + {%- endif %} + {%- endfor %} + + {% if detailed_message.HasOneOf == "true" %} + auto oneof_type = input.{{detailed_message.message.ListFields()[-1][1][0].name}}_case(); + + switch(oneof_type) + { + {%- for field in detailed_message.message.field -%} + {%- if field.HasField("oneof_index") -%} + {%- set splitTypeName = field.type_name.split('.') %} + {%- set EnumName = field.name.replace("_", " ").title().replace(" ", "") %} + case {{detailed_message.message.name}}::k{{EnumName}}: + { + {%- if field.type == 11 %} + gen3_actuator::{{splitTypeName[4]}} temp; + ToRosData(input.{{field.name}}(), temp); + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(temp); + {%- elif field.type == 14 %} + output.oneof_{{detailed_message.message.ListFields()[-1][1][0].name}}.{{field.name}}.push_back(input.{{field.name}}()); + {% endif %} + break; + } + {% endif %} + {%- endfor %} + } + {% endif -%} + + return 0; +} +{% endfor %} diff --git a/kortex_vision_config_driver/templates/ros_converter.h.jinja2 b/kortex_vision_config_driver/templates/ros_converter.h.jinja2 new file mode 100644 index 00000000..ff3e5df8 --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_converter.h.jinja2 @@ -0,0 +1,52 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2018 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +/* + * This file has been auto-generated and should not be modified. + */ + +#ifndef _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ +#define _KORTEX_{{currentPackageName}}ROS_CONVERTER_H_ + +#include "ros/ros.h" + +#include +#include +#include +#include +#include + +#include +#include +#include <{{currentFilename}}.pb.h> + +#include +#include + +#include +#include + +#include +#include +#include +#include + +{% for detailed_message in item -%} +#include "kortex_vision_config_driver/{{detailed_message.message.name}}.h" +{% endfor %} + +using namespace {{currentNamespace}}; + +{% for detailed_message in item -%} +int ToRosData({{detailed_message.message.name}} input, kortex_vision_config_driver::{{detailed_message.message.name}} &output); +{% endfor %} +#endif \ No newline at end of file diff --git a/kortex_vision_config_driver/templates/ros_enum.jinja2 b/kortex_vision_config_driver/templates/ros_enum.jinja2 new file mode 100644 index 00000000..164146fc --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_enum.jinja2 @@ -0,0 +1,3 @@ +{% for member in item.value %} +uint32 {{member.name}} = {{member.number}} +{% endfor %} diff --git a/kortex_vision_config_driver/templates/ros_message.jinja2 b/kortex_vision_config_driver/templates/ros_message.jinja2 new file mode 100644 index 00000000..167f685f --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_message.jinja2 @@ -0,0 +1,44 @@ +{%- for member in item.field -%} +{%- if not member.HasField("oneof_index") -%} +{%- if member.type == 9 %} {# TYPE_STRING #} +string{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 12 %} {# TYPE_BYTES #} +uint8[] {{member.name}} +{%- elif member.type == 1 %} {# TYPE_DOUBLE #} +float64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 7 %} {# TYPE_FIXED32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 6 %} {# TYPE_FIXED64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 2 %} {# TYPE_FLOAT #} +float32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 5 %} {# TYPE_INT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 3 %} {# TYPE_INT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 15 %} {# TYPE_SFIXED32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 16 %} {# TYPE_SFIXED64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 17 %} {# TYPE_SINT32 #} +int32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 18 %} {# TYPE_SINT64 #} +int64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 13 %} {# TYPE_UINT32 #} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 4 %} {# TYPE_UINT64 #} +uint64{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 14 -%} {# TYPE_ENUM #} +{% set list1 = member.type_name.split('.') %} +uint32{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- elif member.type == 8 %} {# TYPE_BOOL #} +bool {{member.name}} +{%- elif member.type == 11 %}{# TYPE MESSAGE #} +{% set list1 = member.type_name.split('.') %} +{{list1|last}}{%- if member.label == 3 -%}[]{% endif %} {{member.name}} +{%- endif -%} +{%- endif -%} +{%- endfor -%} +{%- if HasOneOf %} +{{item.name}}_{{item.ListFields()[-1][1][0].name}} oneof_{{item.ListFields()[-1][1][0].name}} +{%- endif -%} \ No newline at end of file diff --git a/kortex_vision_config_driver/templates/ros_oneof.jinja2 b/kortex_vision_config_driver/templates/ros_oneof.jinja2 new file mode 100644 index 00000000..4fac302a --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_oneof.jinja2 @@ -0,0 +1,9 @@ +{%- for member in item.field -%} +{% if member.HasField("oneof_index") %} +{% if member.type == 11 %} +{% set list1 = member.type_name.split('.') %}{{list1[4]}}[] {{member.name}} +{%- else -%} +uint32[] {{member.name}} +{%- endif -%} +{%- endif -%} +{% endfor %} \ No newline at end of file diff --git a/kortex_vision_config_driver/templates/ros_service.jinja2 b/kortex_vision_config_driver/templates/ros_service.jinja2 new file mode 100644 index 00000000..cc015cf8 --- /dev/null +++ b/kortex_vision_config_driver/templates/ros_service.jinja2 @@ -0,0 +1,5 @@ +{% set split_input_type = item.input_type.split('.') %} +{%- set split_output_type = item.output_type.split('.') -%} +{{split_input_type[4]}} input +--- +{{split_output_type[4]}} output \ No newline at end of file diff --git a/readme.md b/readme.md new file mode 100644 index 00000000..bb967b68 --- /dev/null +++ b/readme.md @@ -0,0 +1,88 @@ +# ROS KORTEX +The official ROS package to interact with Kortex and its related products. It is built on top of the Kortex API. Documentation of the Kortex API can be found in the Kortex [repository](https://github.com/Kinovarobotics/kortex). + + + + +1. [Content](#content) + 1. [kortex driver](#kortex-driver) + 1. [kortex actuator](#kortex-actuator) + 1. [kortex device manager](#kortex-device-manager) + 1. [kortex vision module driver](#kortex-vision-module-driver) + 1. [kortex examples](#kortex-examples) + 1. [kortex description](#kortex-description) + 1. [kortex api](#kortex-api) +1. [Setup](#setup) + 1. [Install protobuf](#install-protobuf) +1. [kortex gazebo](#kortex-gazebo) +1. [kortex moveit](#kortex-moveit) + + + + +## Content + +### kortex driver +This package is a ROS node that allows communication with a robotic base from a Gen3 robot. For more details, please read the [documentation](kortex_driver/readme.md) from inside the package. Use this package if you want to: + +* Change some basic configuration on the robot. +* Move the robot in the Cartesian space. +* Move the robot in the Joint space. +* Activate the admittance mode. +* Move the robot using the **LOW\_LEVEL**\ (1 KHz\) control mode. +* Move the robot using the **LOW\_LEVEL\_BYPASS**\ mode. +* Access the cyclic data sporadically. + + +### kortex actuator +This package is a ROS node that allows a direct communication with a Gen3 actuator. A direct communication means that either the computer that is running the node has direct IP connectivity with the actuator or is connected to a robot and using the device routing system to reach the actuator. A more detailed [documentation](kortex_actuator_driver/readme.md) can be found inside the package. Use this package if you want to: + +* Change an advance configuration setting on an actuator. +* Move an actuator using the cyclic data (1 KHz). + + +### kortex device manager +This package is a ROS node that allows basic communication with every devices supported by the kortex framework. A more detailed [documentation](kortex_device_manager/readme.md) can be found inside the package. Use this package if you want to: + +* List all the devices available on a specific Gen3 robot. +* Retrieve some generic informations on a specific device. +* Get the firmware version of a specific device. +* Get the serial number of a device. +* Set IPv4 settings on a device. +* Get safety informations of a device. + + +### kortex vision module driver +This package is a ROS node that allows direct communication with a Gen3 vision module. A direct communication means that either the computer running the node has an ethernet cable directly connected to a vision module or that it is connected to a robot using the device routing system. A more detailed [documentation](kortex_vision_config_driver/readme.md) can be found inside the package. Use this package if you want to: + +* Change a configuration setting on a vision module. +* Get informations about the configuration settings of a vision module. + + +### kortex examples +This package holds all the examples needed to understands that basics of ros_kortex. All examples are written in both c++ and python. A more detailed [documentation](kortex_examples/readme.md) can be found inside the package. + + +### kortex description +This package contains the URDF and the STL of a complete Gen3 robot. A more detailed [documentation](kortex_description/readme.md) can be found inside the package. + + +### kortex api +This package contains all the header files and the libraries needed by the kortex C++ API. A more detailed [documentation](kortex_api/readme.md) can be found inside the package. + + +## Setup + +### Install protobuf +Protobuf compiler is needed if you want to re-generate the code of a package. + +1. git clone https://github.com/protocolbuffers/protobuf --branch 3.5.1.1 (you absolutely need to use this specific version) +2. Follow these [instructions](https://github.com/protocolbuffers/protobuf/blob/master/src/README.md) to build and install protobuf and its compiler. + + +## kortex gazebo +This package is not completed yet but will be available in a future version. + + +## kortex moveit +This package is not completed yet but will be available in a future version.