first commit

.gitignore

@@ -0,0 +1 @@
+

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.

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)
+

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)

kortex_actuator_driver/kortex_actuator_driver.sh

@@ -0,0 +1,4 @@
+#!/bin/sh
+
+python3 -u RosGeneration.py
+

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

kortex_actuator_driver/msg/AxisOffsets.msg

@@ -0,0 +1,3 @@
+
+float32 absolute_offset 
+float32 relative_offset

kortex_actuator_driver/msg/AxisPosition.msg

@@ -0,0 +1,2 @@
+
+float32 position

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