From 8bd9d50bb104a163edd5cb55e04a02672db88efd Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 7 Sep 2020 16:44:11 -0700 Subject: [PATCH 1/5] wip --- blue_hardware_drivers/CMakeLists.txt | 11 +- blue_hardware_drivers/cmake/FindCython.cmake | 45 +++ .../cmake/ReplicatePythonSourceTree.cmake | 4 + blue_hardware_drivers/cmake/UseCython.cmake | 287 ++++++++++++++++++ ...ollerClient.h => BLDCControllerClient.hpp} | 0 .../blue_hardware_drivers/BLDCDriver.h | 2 +- .../scripts/bldc_driver_frequency_test.py | 83 +---- blue_hardware_drivers/setup.py | 13 + .../src/BLDCControllerClient.cpp | 2 +- .../BLDCDriver_communication_freq_test.cpp | 2 +- .../src/cython/BLDCControllerClient.pyx | 113 +++++++ .../src/cython/CMakeLists.txt | 18 ++ 12 files changed, 499 insertions(+), 81 deletions(-) create mode 100644 blue_hardware_drivers/cmake/FindCython.cmake create mode 100644 blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake create mode 100644 blue_hardware_drivers/cmake/UseCython.cmake rename blue_hardware_drivers/include/blue_hardware_drivers/{BLDCControllerClient.h => BLDCControllerClient.hpp} (100%) create mode 100644 blue_hardware_drivers/setup.py create mode 100644 blue_hardware_drivers/src/cython/BLDCControllerClient.pyx create mode 100644 blue_hardware_drivers/src/cython/CMakeLists.txt diff --git a/blue_hardware_drivers/CMakeLists.txt b/blue_hardware_drivers/CMakeLists.txt index db0affa3..e66a4d47 100644 --- a/blue_hardware_drivers/CMakeLists.txt +++ b/blue_hardware_drivers/CMakeLists.txt @@ -80,7 +80,7 @@ install(TARGETS BLDCDriver_communication_freq_test ## Mark cpp header files for installation install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" + FILES_MATCHING PATTERN "*.h*" ) ## Mark Python scripts for installation @@ -92,3 +92,12 @@ install(PROGRAMS scripts/comms.py scripts/bldc_driver.py scripts/bldc_driver_fre install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) + +# set CMake module path for Cython +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +include(UseCython) +add_custom_target(ReplicatePythonSourceTree ALL ${CMAKE_COMMAND} -P + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/ReplicatePythonSourceTree.cmake + ${CMAKE_CURRENT_BINARY_DIR} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(src/cython) diff --git a/blue_hardware_drivers/cmake/FindCython.cmake b/blue_hardware_drivers/cmake/FindCython.cmake new file mode 100644 index 00000000..7d28eb14 --- /dev/null +++ b/blue_hardware_drivers/cmake/FindCython.cmake @@ -0,0 +1,45 @@ +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_EXECUTABLE +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if( PYTHONINTERP_FOUND ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat + HINTS ${_python_path} + ) +else() + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + ) +endif() + + +include( FindPackageHandleStandardArgs ) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE ) + +mark_as_advanced( CYTHON_EXECUTABLE ) + diff --git a/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake b/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake new file mode 100644 index 00000000..d308cce7 --- /dev/null +++ b/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake @@ -0,0 +1,4 @@ +# Note: when executed in the build dir, then CMAKE_CURRENT_SOURCE_DIR is the +# build dir. +file( COPY setup.py src test bin DESTINATION "${CMAKE_ARGV3}" + FILES_MATCHING PATTERN "*.py" ) diff --git a/blue_hardware_drivers/cmake/UseCython.cmake b/blue_hardware_drivers/cmake/UseCython.cmake new file mode 100644 index 00000000..f432c890 --- /dev/null +++ b/blue_hardware_drivers/cmake/UseCython.cmake @@ -0,0 +1,287 @@ +# Define a function to create Cython modules. +# +# For more information on the Cython project, see http://cython.org/. +# "Cython is a language that makes writing C extensions for the Python language +# as easy as Python itself." +# +# This file defines a CMake function to build a Cython Python module. +# To use it, first include this file. +# +# include( UseCython ) +# +# Then call cython_add_module to create a module. +# +# cython_add_module( ... ) +# +# To create a standalone executable, the function +# +# cython_add_standalone_executable( [MAIN_MODULE src1] ... ) +# +# To avoid dependence on Python, set the PYTHON_LIBRARY cache variable to point +# to a static library. If a MAIN_MODULE source is specified, +# the "if __name__ == '__main__':" from that module is used as the C main() method +# for the executable. If MAIN_MODULE, the source with the same basename as +# is assumed to be the MAIN_MODULE. +# +# Where is the name of the resulting Python module and +# ... are source files to be compiled into the module, e.g. *.pyx, +# *.py, *.c, *.cxx, etc. A CMake target is created with name . This can +# be used for target_link_libraries(), etc. +# +# The sample paths set with the CMake include_directories() command will be used +# for include directories to search for *.pxd when running the Cython complire. +# +# Cache variables that effect the behavior include: +# +# CYTHON_ANNOTATE +# CYTHON_NO_DOCSTRINGS +# CYTHON_FLAGS +# +# Source file properties that effect the build process are +# +# CYTHON_IS_CXX +# +# If this is set of a *.pyx file with CMake set_source_files_properties() +# command, the file will be compiled as a C++ file. +# +# See also FindCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Configuration options. +set( CYTHON_ANNOTATE OFF + CACHE BOOL "Create an annotated .html file when compiling *.pyx." ) +set( CYTHON_NO_DOCSTRINGS OFF + CACHE BOOL "Strip docstrings from the compiled module." ) +set( CYTHON_FLAGS "" CACHE STRING + "Extra flags to the cython compiler." ) +mark_as_advanced( CYTHON_ANNOTATE CYTHON_NO_DOCSTRINGS CYTHON_FLAGS ) + +find_package( Cython REQUIRED ) +find_package( PythonLibs REQUIRED ) + +set( CYTHON_CXX_EXTENSION "cxx" ) +set( CYTHON_C_EXTENSION "c" ) + +# Create a *.c or *.cxx file from a *.pyx file. +# Input the generated file basename. The generate file will put into the variable +# placed in the "generated_file" argument. Finally all the *.py and *.pyx files. +function( compile_pyx _name generated_file ) + # Default to assuming all files are C. + set( cxx_arg "" ) + set( extension ${CYTHON_C_EXTENSION} ) + set( pyx_lang "C" ) + set( comment "Compiling Cython C source for ${_name}..." ) + + set( cython_include_directories "" ) + set( pxd_dependencies "" ) + set( c_header_dependencies "" ) + set( pyx_locations "" ) + + foreach( pyx_file ${ARGN} ) + get_filename_component( pyx_file_basename "${pyx_file}" NAME_WE ) + + # Determine if it is a C or C++ file. + get_source_file_property( property_is_cxx ${pyx_file} CYTHON_IS_CXX ) + if( ${property_is_cxx} ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython CXX source for ${_name}..." ) + endif() + + # Get the include directories. + get_source_file_property( pyx_location ${pyx_file} LOCATION ) + get_filename_component( pyx_path ${pyx_location} PATH ) + get_directory_property( cmake_include_directories DIRECTORY ${pyx_path} INCLUDE_DIRECTORIES ) + list( APPEND cython_include_directories ${cmake_include_directories} ) + list( APPEND pyx_locations "${pyx_location}" ) + + # Determine dependencies. + # Add the pxd file will the same name as the given pyx file. + unset( corresponding_pxd_file CACHE ) + find_file( corresponding_pxd_file ${pyx_file_basename}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} + NO_DEFAULT_PATH ) + if( corresponding_pxd_file ) + list( APPEND pxd_dependencies "${corresponding_pxd_file}" ) + endif() + + # pxd files to check for additional dependencies. + set( pxds_to_check "${pyx_file}" "${pxd_dependencies}" ) + set( pxds_checked "" ) + set( number_pxds_to_check 1 ) + while( ${number_pxds_to_check} GREATER 0 ) + foreach( pxd ${pxds_to_check} ) + list( APPEND pxds_checked "${pxd}" ) + list( REMOVE_ITEM pxds_to_check "${pxd}" ) + + # check for C header dependencies + file( STRINGS "${pxd}" extern_from_statements + REGEX "cdef[ ]+extern[ ]+from.*$" ) + foreach( statement ${extern_from_statements} ) + # Had trouble getting the quote in the regex + string( REGEX REPLACE "cdef[ ]+extern[ ]+from[ ]+[\"]([^\"]+)[\"].*" "\\1" header "${statement}" ) + unset( header_location CACHE ) + find_file( header_location ${header} PATHS ${cmake_include_directories} ) + if( header_location ) + list( FIND c_header_dependencies "${header_location}" header_idx ) + if( ${header_idx} LESS 0 ) + list( APPEND c_header_dependencies "${header_location}" ) + endif() + endif() + endforeach() + + # check for pxd dependencies + + # Look for cimport statements. + set( module_dependencies "" ) + file( STRINGS "${pxd}" cimport_statements REGEX cimport ) + foreach( statement ${cimport_statements} ) + if( ${statement} MATCHES from ) + string( REGEX REPLACE "from[ ]+([^ ]+).*" "\\1" module "${statement}" ) + else() + string( REGEX REPLACE "cimport[ ]+([^ ]+).*" "\\1" module "${statement}" ) + endif() + list( APPEND module_dependencies ${module} ) + endforeach() + list( REMOVE_DUPLICATES module_dependencies ) + # Add the module to the files to check, if appropriate. + foreach( module ${module_dependencies} ) + unset( pxd_location CACHE ) + find_file( pxd_location ${module}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} NO_DEFAULT_PATH ) + if( pxd_location ) + list( FIND pxds_checked ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( FIND pxds_to_check ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( APPEND pxds_to_check ${pxd_location} ) + list( APPEND pxd_dependencies ${pxd_location} ) + endif() # if it is not already going to be checked + endif() # if it has not already been checked + endif() # if pxd file can be found + endforeach() # for each module dependency discovered + endforeach() # for each pxd file to check + list( LENGTH pxds_to_check number_pxds_to_check ) + endwhile() + endforeach() # pyx_file + + # Set additional flags. + if( CYTHON_ANNOTATE ) + set( annotate_arg "--annotate" ) + endif() + + if( CYTHON_NO_DOCSTRINGS ) + set( no_docstrings_arg "--no-docstrings" ) + endif() + + if( "${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR + "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo" ) + set( cython_debug_arg "--gdb" ) + endif() + + # Include directory arguments. + list( REMOVE_DUPLICATES cython_include_directories ) + set( include_directory_arg "" ) + foreach( _include_dir ${cython_include_directories} ) + set( include_directory_arg ${include_directory_arg} "-I" "${_include_dir}" ) + endforeach() + + # Determining generated file name. + set( _generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.${extension}" ) + set_source_files_properties( ${_generated_file} PROPERTIES GENERATED TRUE ) + set( ${generated_file} ${_generated_file} PARENT_SCOPE ) + + list( REMOVE_DUPLICATES pxd_dependencies ) + list( REMOVE_DUPLICATES c_header_dependencies ) + + # Add the command to run the compiler. + add_custom_command( OUTPUT ${_generated_file} + COMMAND ${CYTHON_EXECUTABLE} + ARGS ${cxx_arg} ${include_directory_arg} + ${annotate_arg} ${no_docstrings_arg} ${cython_debug_arg} ${CYTHON_FLAGS} + --output-file ${_generated_file} ${pyx_locations} + DEPENDS ${pyx_locations} ${pxd_dependencies} + IMPLICIT_DEPENDS ${pyx_lang} ${c_header_dependencies} + COMMENT ${comment} + ) + + # Remove their visibility to the user. + set( corresponding_pxd_file "" CACHE INTERNAL "" ) + set( header_location "" CACHE INTERNAL "" ) + set( pxd_location "" CACHE INTERNAL "" ) +endfunction() + +# cython_add_module( src1 src2 ... srcN ) +# Build the Cython Python module. +function( cython_add_module _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + foreach( _file ${ARGN} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + list( APPEND pyx_module_sources ${_file} ) + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + compile_pyx( ${_name} generated_file ${pyx_module_sources} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + python_add_module( ${_name} ${generated_file} ${other_module_sources} ) + if( APPLE ) + set_target_properties( ${_name} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" ) + else() + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ) + endif() +endfunction() + +include( CMakeParseArguments ) +# cython_add_standalone_executable( _name [MAIN_MODULE src3.py] src1 src2 ... srcN ) +# Creates a standalone executable the given sources. +function( cython_add_standalone_executable _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + set( main_module "" ) + cmake_parse_arguments( cython_arguments "" "MAIN_MODULE" "" ${ARGN} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + foreach( _file ${cython_arguments_UNPARSED_ARGUMENTS} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + get_filename_component( _file_we ${_file} NAME_WE ) + if( "${_file_we}" STREQUAL "${_name}" ) + set( main_module "${_file}" ) + elseif( NOT "${_file}" STREQUAL "${cython_arguments_MAIN_MODULE}" ) + set( PYTHON_MODULE_${_file_we}_static_BUILD_SHARED OFF ) + compile_pyx( "${_file_we}_static" generated_file "${_file}" ) + list( APPEND pyx_module_sources "${generated_file}" ) + endif() + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + + if( cython_arguments_MAIN_MODULE ) + set( main_module ${cython_arguments_MAIN_MODULE} ) + endif() + if( NOT main_module ) + message( FATAL_ERROR "main module not found." ) + endif() + get_filename_component( main_module_we "${main_module}" NAME_WE ) + set( CYTHON_FLAGS ${CYTHON_FLAGS} --embed ) + compile_pyx( "${main_module_we}_static" generated_file ${main_module} ) + add_executable( ${_name} ${generated_file} ${pyx_module_sources} ${other_module_sources} ) + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ${pyx_module_libs} ) +endfunction() diff --git a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.h b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.hpp similarity index 100% rename from blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.h rename to blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.hpp diff --git a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h index 3d8d69e3..1726d801 100644 --- a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h +++ b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h @@ -14,7 +14,7 @@ #include "time.h" #include "blue_msgs/MotorState.h" -#include "blue_hardware_drivers/BLDCControllerClient.h" +#include "blue_hardware_drivers/BLDCControllerClient.hpp" namespace blue_hardware_drivers { diff --git a/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py b/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py index 5730d32f..58d0569f 100755 --- a/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py +++ b/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py @@ -1,15 +1,7 @@ #!/usr/bin/env python -from comms import BLDCControllerClient -import time -import serial -import math -import signal +from blue_hardware_drivers import PyBLDCControllerClient import sys import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 -from std_msgs.msg import Float32 -from comms import * # Example usage: # rosrun blue_hardware_drivers bldc_driver_frequency_test.py 2,42,41,33,16,31,32,14 /dev/ttyUSB1 @@ -24,9 +16,7 @@ def main(): rospy.init_node('freq_publisher', anonymous=True) - motor_ids = [int(x) for x in sys.argv[1].split(",")] - if len(sys.argv) >= 3: port = sys.argv[2] else: @@ -34,73 +24,12 @@ def main(): rospy.loginfo("Testing communication frequency: {} {}".format(motor_ids, port)) - s = serial.Serial(port=port, baudrate=1000000, timeout=0.01) - device = BLDCControllerClient(s) - time.sleep(0.1) - - device.leaveBootloader(motor_ids) - - time.sleep(0.2) - s.flush() - time.sleep(0.1) - - - freq_pub = rospy.Publisher("/freq", Float32, queue_size=1) - msg = Float32() - - last_time = rospy.get_time() - - for id in motor_ids: - success = False - for attempt in range(5): - try: - rospy.loginfo("Calibrating motor %d..." % id) - #calibrations = device.readCalibration([id]) - #print(calibrations) - #device.setZeroAngle([id], [calibrations['angle']]) - #device.setInvertPhases([id], [calibrations['inv']]) - #device.setERevsPerMRev([id], [calibrations['epm']]) - #device.setTorqueConstant([id], [calibrations['torque']]) - #device.setPositionOffset([id], [calibrations['zero']]) - device._ser.read_all() - device.setZeroAngle([id], [1169]) - device.setInvertPhases([id], [1]) - device.setERevsPerMRev([id], [14]) - device.setTorqueConstant([id], [1.45]) - device.setPositionOffset([id], [0.0]) - device.setCurrentControlMode([id]) - #starting_angles[id] = 0.0 - device.writeRegisters([id], [0x1030], [1], [struct.pack(' #include #include diff --git a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp index 72f8cf0f..10d87f1d 100644 --- a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp +++ b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp @@ -4,7 +4,7 @@ #include "std_msgs/Float64.h" #include "std_msgs/Float32.h" #include "sensor_msgs/JointState.h" -#include "blue_hardware_drivers/BLDCControllerClient.h" +#include "blue_hardware_drivers/BLDCControllerClient.hpp" #include #include #include "math.h" diff --git a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx new file mode 100644 index 00000000..c4eef9ba --- /dev/null +++ b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx @@ -0,0 +1,113 @@ +# distutils: language = c++ + +from libcpp cimport bool +import cython +from libc.stdlib cimport malloc, free + +cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp": + cppclass BLDCControllerClient: + BLDCControllerClient() except + + init(str port, int* boards) + + # # queuePacket(int board_id, Packet* packet); + # + # # Initialize Board ID for Disco Bus Protocol + # queueConfirmID(int board_id); + # queueEnumerate(int board_id); + # getEnumerateResponse(int board_id, int* response_id); + # + # # Program Counter Adjustments + # # queueLeaveBootloader(int board_id, uint32_t jump_addr); + # + # # Calibration Setup + # # queueSetTimeout(int board_id, uint16_t value); + # # queueSetControlMode(int board_id, comm_ctrl_mode_t control_mode); + # # queueSetZeroAngle(int board_id, uint16_t value); + # # queueSetERevsPerMRev(int board_id, uint8_t value); + # # queueSetInvertPhases(int board_id, uint8_t value); + # queueSetTorqueConstant(int board_id, float value); + # queueSetPositionOffset(int board_id, float value); + # queueSetEACScale(int board_id, float value); + # queueSetEACOffset(int board_id, float value); + # # queueSetEACTable(int board_id, size_t start_index, uint8_t *values, size_t count); + # queueSetDirectCurrentControllerKp(int board_id, float value); + # queueSetDirectCurrentControllerKi(int board_id, float value); + # queueSetQuadratureCurrentControllerKp(int board_id, float value); + # queueSetQuadratureCurrentControllerKi(int board_id, float value); + # queueSetVelocityControllerKp(int board_id, float value); + # queueSetVelocityControllerKi(int board_id, float value); + # queueSetPositionControllerKp(int board_id, float value); + # queueSetPositionControllerKi(int board_id, float value); + # queueSetIAOffset(int board_id, float value); + # queueSetIBOffset(int board_id, float value); + # queueSetICOffset(int board_id, float value); + # + # # Drive Commands + # queueSetCommand(int board_id, float value); + # queueSetPosCommand(int board_id, float position, float feedforward); + # queueGetRotorPosition(int board_id); + # queueSetCommandAndGetRotorPosition(int board_id, float value); + # queueSetPositionAndGetRotorPosition(int board_id, float value); + # queueGetState(int board_id); + # queueSetCommandAndGetState(int board_id, float value); + # queueSetPosCommandAndGetState(int board_id, float position, float feedforward); + + # Init Motor State Commands + # queueSetRevolutions(int board_id, int16_t value); + + # Send queued packets and receive from boards + exchange(); + + # Remove all items currently in the queue + clearQueue(); + + # Send all boards to bootloader + resetBoards(); + + # Clear the RS485 Buffer + resetBuffer(); + + # # Check if a watchdog reset has occurred on a given board + # bool checkWDGRST(int board_id); + # bool checkTimeout(int board_id); + # queueClearWDGRST(int board_id); + # + # # Result Commands + # resultGetRotorPosition(int board_id, float* result); + # # resultGetState(int board_id, float* position, float* velocity, float* di, float* qi, float* voltage, float* temp, int32_t* acc_x, int32_t* acc_y, int32_t* acc_z); + # + # # Setup/Programming Commands + # bool initMotor(int board_id); + +cdef class PyBLDCControllerClient: + cdef BLDCControllerClient _bldc_client + + def __init__( + PyBLDCControllerClient self, + port, + boards + ): + self._bldc_client = BLDCControllerClient() + cdef int* c_boards + c_boards = malloc(len(boards) * sizeof(int)) + for i in range(len(boards)): + c_boards[i] = boards[i] + self._bldc_client.init(port, c_boards) + + # def queue_get_rotor_position(self, board_id): + # self._bldc_client.queueGetRotorPosition(board_id) + + def exchange(self): + self._bldc_client.exchange() + + def clear_queue(self): + self._bldc_client.clearQueue() + + def reset_boards(self): + self._bldc_client.resetBoards() + + def reset_buffer(self): + self._bldc_client.resetBuffer() + + # def result_get_rotor_position(self, board_id, result): + # self._bldc_client.resultGetRotorPosition(board_id, result) diff --git a/blue_hardware_drivers/src/cython/CMakeLists.txt b/blue_hardware_drivers/src/cython/CMakeLists.txt new file mode 100644 index 00000000..589ab8a0 --- /dev/null +++ b/blue_hardware_drivers/src/cython/CMakeLists.txt @@ -0,0 +1,18 @@ +# CAUTION: +# - You can specify only one .pyx file for each cython module. +# - The name of the module and the .pyx file should be identical. +cython_add_module(BLDCControllerClient BLDCControllerClient.pyx ../BLDCControllerClient.cpp) + +# The library (libros_cython_example.so) is created in upper CMakeLists.txt +target_link_libraries(BLDCControllerClient blue_hardware_drivers) + +# Set the targets to be copied in the python package directry. +set_target_properties(BLDCControllerClient + PROPERTIES + LIBRARY_OUTPUT_DIRECTORY + ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}) + +#install(TARGETS add_two_ints sub_two_ints +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + From 5e10fe97ae20d8ffd7e1f667d01dba5487608d89 Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 7 Sep 2020 18:53:55 -0700 Subject: [PATCH 2/5] compile --- blue_hardware_drivers/cmake/UseCython.cmake | 6 ++-- .../src/cython/BLDCControllerClient.pyx | 30 ++++++++++++------- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/blue_hardware_drivers/cmake/UseCython.cmake b/blue_hardware_drivers/cmake/UseCython.cmake index f432c890..0eb13c50 100644 --- a/blue_hardware_drivers/cmake/UseCython.cmake +++ b/blue_hardware_drivers/cmake/UseCython.cmake @@ -82,9 +82,9 @@ set( CYTHON_C_EXTENSION "c" ) # placed in the "generated_file" argument. Finally all the *.py and *.pyx files. function( compile_pyx _name generated_file ) # Default to assuming all files are C. - set( cxx_arg "" ) - set( extension ${CYTHON_C_EXTENSION} ) - set( pyx_lang "C" ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) set( comment "Compiling Cython C source for ${_name}..." ) set( cython_include_directories "" ) diff --git a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx index c4eef9ba..4a526e0f 100644 --- a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx +++ b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx @@ -1,13 +1,20 @@ # distutils: language = c++ +# cython: c_string_type=unicode, c_string_encoding=utf8 from libcpp cimport bool import cython from libc.stdlib cimport malloc, free +from libcpp.string cimport string +from libcpp.vector cimport vector -cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp": +# cdef extern from "blue_hardware_drivers/comms_defs.h" namespace "blue_hardware_drivers": + # cdef comm_id_t + +ctypedef unsigned char comm_id_t + +cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp" namespace "blue_hardware_drivers": cppclass BLDCControllerClient: - BLDCControllerClient() except + - init(str port, int* boards) + BLDCControllerClient(string port, vector[comm_id_t] boards) except + # # queuePacket(int board_id, Packet* packet); # @@ -80,23 +87,24 @@ cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp": # bool initMotor(int board_id); cdef class PyBLDCControllerClient: - cdef BLDCControllerClient _bldc_client + cdef BLDCControllerClient*c_bldc_client - def __init__( + def __cinit__( PyBLDCControllerClient self, - port, + string c_port, boards ): - self._bldc_client = BLDCControllerClient() - cdef int* c_boards - c_boards = malloc(len(boards) * sizeof(int)) + cdef vector[comm_id_t] c_boards for i in range(len(boards)): - c_boards[i] = boards[i] - self._bldc_client.init(port, c_boards) + c_boards.push_back(i) + self.c_bldc_client = new BLDCControllerClient(c_port, c_boards) # def queue_get_rotor_position(self, board_id): # self._bldc_client.queueGetRotorPosition(board_id) + def __dealloc__(self): + del self.c_bldc_client + def exchange(self): self._bldc_client.exchange() From 51e834bfc70919c9527812ea08934a253c089c88 Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 7 Sep 2020 19:01:24 -0700 Subject: [PATCH 3/5] test script --- .../src/cython/BLDCControllerClient.pyx | 105 +++++++++--------- 1 file changed, 53 insertions(+), 52 deletions(-) diff --git a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx index 4a526e0f..0efafe91 100644 --- a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx +++ b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx @@ -16,51 +16,49 @@ cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp" namespace "blu cppclass BLDCControllerClient: BLDCControllerClient(string port, vector[comm_id_t] boards) except + - # # queuePacket(int board_id, Packet* packet); - # # # Initialize Board ID for Disco Bus Protocol - # queueConfirmID(int board_id); - # queueEnumerate(int board_id); - # getEnumerateResponse(int board_id, int* response_id); - # + queueConfirmID(comm_id_t board_id); + queueEnumerate(comm_id_t board_id); + getEnumerateResponse(comm_id_t board_id, int* response_id); + # # Program Counter Adjustments - # # queueLeaveBootloader(int board_id, uint32_t jump_addr); - # - # # Calibration Setup - # # queueSetTimeout(int board_id, uint16_t value); - # # queueSetControlMode(int board_id, comm_ctrl_mode_t control_mode); - # # queueSetZeroAngle(int board_id, uint16_t value); - # # queueSetERevsPerMRev(int board_id, uint8_t value); - # # queueSetInvertPhases(int board_id, uint8_t value); - # queueSetTorqueConstant(int board_id, float value); - # queueSetPositionOffset(int board_id, float value); - # queueSetEACScale(int board_id, float value); - # queueSetEACOffset(int board_id, float value); - # # queueSetEACTable(int board_id, size_t start_index, uint8_t *values, size_t count); - # queueSetDirectCurrentControllerKp(int board_id, float value); - # queueSetDirectCurrentControllerKi(int board_id, float value); - # queueSetQuadratureCurrentControllerKp(int board_id, float value); - # queueSetQuadratureCurrentControllerKi(int board_id, float value); - # queueSetVelocityControllerKp(int board_id, float value); - # queueSetVelocityControllerKi(int board_id, float value); - # queueSetPositionControllerKp(int board_id, float value); - # queueSetPositionControllerKi(int board_id, float value); - # queueSetIAOffset(int board_id, float value); - # queueSetIBOffset(int board_id, float value); - # queueSetICOffset(int board_id, float value); - # + # queueLeaveBootloader(comm_id_t board_id, uint32_t jump_addr); + + # Calibration Setup + # queueSetTimeout(comm_id_t board_id, uint16_t value); + # queueSetControlMode(comm_id_t board_id, comm_ctrl_mode_t control_mode); + # queueSetZeroAngle(comm_id_t board_id, uint16_t value); + # queueSetERevsPerMRev(comm_id_t board_id, uint8_t value); + # queueSetInvertPhases(comm_id_t board_id, uint8_t value); + # queueSetTorqueConstant(comm_id_t board_id, float value); + # queueSetPositionOffset(comm_id_t board_id, float value); + # queueSetEACScale(comm_id_t board_id, float value); + # queueSetEACOffset(comm_id_t board_id, float value); + # queueSetEACTable(comm_id_t board_id, size_t start_index, uint8_t *values, size_t count); + queueSetDirectCurrentControllerKp(comm_id_t board_id, float value); + queueSetDirectCurrentControllerKi(comm_id_t board_id, float value); + queueSetQuadratureCurrentControllerKp(comm_id_t board_id, float value); + queueSetQuadratureCurrentControllerKi(comm_id_t board_id, float value); + queueSetVelocityControllerKp(comm_id_t board_id, float value); + queueSetVelocityControllerKi(comm_id_t board_id, float value); + queueSetPositionControllerKp(comm_id_t board_id, float value); + queueSetPositionControllerKi(comm_id_t board_id, float value); + # queueSetIAOffset(comm_id_t board_id, float value); + # queueSetIBOffset(comm_id_t board_id, float value); + # queueSetICOffset(comm_id_t board_id, float value); + # # Drive Commands - # queueSetCommand(int board_id, float value); - # queueSetPosCommand(int board_id, float position, float feedforward); - # queueGetRotorPosition(int board_id); - # queueSetCommandAndGetRotorPosition(int board_id, float value); - # queueSetPositionAndGetRotorPosition(int board_id, float value); - # queueGetState(int board_id); - # queueSetCommandAndGetState(int board_id, float value); - # queueSetPosCommandAndGetState(int board_id, float position, float feedforward); + queueSetCommand(comm_id_t board_id, float value); + queueSetPosCommand(comm_id_t board_id, float position, float feedforward); + queueGetRotorPosition(comm_id_t board_id); + queueSetCommandAndGetRotorPosition(comm_id_t board_id, float value); + queueSetPositionAndGetRotorPosition(comm_id_t board_id, float value); + queueGetState(comm_id_t board_id); + queueSetCommandAndGetState(comm_id_t board_id, float value); + queueSetPosCommandAndGetState(comm_id_t board_id, float position, float feedforward); # Init Motor State Commands - # queueSetRevolutions(int board_id, int16_t value); + # queueSetRevolutions(comm_id_t board_id, int16_t value); # Send queued packets and receive from boards exchange(); @@ -75,16 +73,16 @@ cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp" namespace "blu resetBuffer(); # # Check if a watchdog reset has occurred on a given board - # bool checkWDGRST(int board_id); - # bool checkTimeout(int board_id); - # queueClearWDGRST(int board_id); - # + bool checkWDGRST(comm_id_t board_id); + bool checkTimeout(comm_id_t board_id); + queueClearWDGRST(comm_id_t board_id); + # # Result Commands - # resultGetRotorPosition(int board_id, float* result); - # # resultGetState(int board_id, float* position, float* velocity, float* di, float* qi, float* voltage, float* temp, int32_t* acc_x, int32_t* acc_y, int32_t* acc_z); - # + resultGetRotorPosition(comm_id_t board_id, float* result); + # resultGetState(comm_id_t board_id, float* position, float* velocity, float* di, float* qi, float* voltage, float* temp, int32_t* acc_x, int32_t* acc_y, int32_t* acc_z); + # # Setup/Programming Commands - # bool initMotor(int board_id); + bool initMotor(comm_id_t board_id); cdef class PyBLDCControllerClient: cdef BLDCControllerClient*c_bldc_client @@ -99,8 +97,8 @@ cdef class PyBLDCControllerClient: c_boards.push_back(i) self.c_bldc_client = new BLDCControllerClient(c_port, c_boards) - # def queue_get_rotor_position(self, board_id): - # self._bldc_client.queueGetRotorPosition(board_id) + def queue_get_rotor_position(self, comm_id_t board_id): + self._bldc_client.queueGetRotorPosition(board_id) def __dealloc__(self): del self.c_bldc_client @@ -117,5 +115,8 @@ cdef class PyBLDCControllerClient: def reset_buffer(self): self._bldc_client.resetBuffer() - # def result_get_rotor_position(self, board_id, result): - # self._bldc_client.resultGetRotorPosition(board_id, result) + def init_motor(self, board_id): + self._bldc_client.initMotor(board_id) + + def result_get_rotor_position(self, board_id, result): + self._bldc_client.resultGetRotorPosition(board_id, result) From 84e2ff0dc6ac23431e702b7c99d662bb3dffe460 Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 7 Sep 2020 19:02:34 -0700 Subject: [PATCH 4/5] add dependency --- blue_hardware_drivers/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/blue_hardware_drivers/package.xml b/blue_hardware_drivers/package.xml index 1ebbf9d2..418ad252 100644 --- a/blue_hardware_drivers/package.xml +++ b/blue_hardware_drivers/package.xml @@ -17,6 +17,7 @@ roscpp rospy + cython sensor_msgs serial std_msgs From fe461c458d9525b5cb7171fb82eff08773ce22bf Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 7 Sep 2020 19:44:37 -0700 Subject: [PATCH 5/5] minor --- blue_bringup/package.xml | 2 ++ blue_hardware_drivers/setup.py | 2 +- blue_hardware_drivers/src/cython/BLDCControllerClient.pyx | 3 --- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index 977d5f06..ab77af44 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -11,6 +11,8 @@ Philipp Wu catkin + roscpp + rospy blue_controller_manager blue_msgs diff --git a/blue_hardware_drivers/setup.py b/blue_hardware_drivers/setup.py index e3a8ff9e..791fc981 100644 --- a/blue_hardware_drivers/setup.py +++ b/blue_hardware_drivers/setup.py @@ -5,7 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['ros_cython_example'], + packages=['blue_hardware_drivers'], package_dir={'': 'src'}, requires=['std_msgs', 'rospy'] ) diff --git a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx index 0efafe91..e708a137 100644 --- a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx +++ b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx @@ -7,9 +7,6 @@ from libc.stdlib cimport malloc, free from libcpp.string cimport string from libcpp.vector cimport vector -# cdef extern from "blue_hardware_drivers/comms_defs.h" namespace "blue_hardware_drivers": - # cdef comm_id_t - ctypedef unsigned char comm_id_t cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp" namespace "blue_hardware_drivers":