From ce44f00cfe8418319551849addab62e1ecdde199 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 09:29:13 +0900 Subject: [PATCH 1/4] use package format3, and use python3-pykdl / python_orocos_kdl depends on ROS_PYTHON_VERSION, see http://wiki.ros.org/noetic/Migration#Use_Orocos_KDL_and_BFL_rosdep_keys --- dynamixel_driver/package.xml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/dynamixel_driver/package.xml b/dynamixel_driver/package.xml index 09096b0..bf1873d 100644 --- a/dynamixel_driver/package.xml +++ b/dynamixel_driver/package.xml @@ -1,4 +1,4 @@ - + dynamixel_driver 0.4.1 @@ -22,9 +22,10 @@ catkin - rospy - diagnostic_msgs - python-serial + rospy + diagnostic_msgs + python-serial + python3-serial From 927249f65538e4cd79071a7e7919648ffa3bca1a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 09:29:49 +0900 Subject: [PATCH 2/4] run 2to3 -w -fprint . --- dynamixel_driver/scripts/change_id.py | 14 ++++----- dynamixel_driver/scripts/info_dump.py | 20 ++++++------ dynamixel_driver/scripts/set_servo_config.py | 32 ++++++++++---------- dynamixel_driver/scripts/set_torque.py | 8 ++--- 4 files changed, 37 insertions(+), 37 deletions(-) diff --git a/dynamixel_driver/scripts/change_id.py b/dynamixel_driver/scripts/change_id.py index a5ba515..fba37ae 100755 --- a/dynamixel_driver/scripts/change_id.py +++ b/dynamixel_driver/scripts/change_id.py @@ -71,16 +71,16 @@ try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError, soe: - print 'ERROR:', soe + print('ERROR:', soe) else: - print 'Changing motor id from %d to %d...' %(old_id, new_id), + print('Changing motor id from %d to %d...' %(old_id, new_id), end=' ') if dxl_io.ping(old_id): dxl_io.set_id(old_id, new_id) - print ' done' - print 'Verifying new id...' , + print(' done') + print('Verifying new id...', end=' ') if dxl_io.ping(new_id): - print ' done' + print(' done') else: - print 'ERROR: The motor did not respond to a ping to its new id.' + print('ERROR: The motor did not respond to a ping to its new id.') else: - print 'ERROR: The specified motor did not respond to id %d. Make sure to specify the correct baudrate.' % old_id + print('ERROR: The specified motor did not respond to id %d. Make sure to specify the correct baudrate.' % old_id) diff --git a/dynamixel_driver/scripts/info_dump.py b/dynamixel_driver/scripts/info_dump.py index c104f00..54fa3d6 100755 --- a/dynamixel_driver/scripts/info_dump.py +++ b/dynamixel_driver/scripts/info_dump.py @@ -55,7 +55,7 @@ def print_data(values): ''' Takes a dictionary with all the motor values and does a formatted print. ''' if values['freespin']: - print '''\ + print('''\ Motor %(id)d is connected: Freespin: True Model ------------------- %(model)s @@ -64,9 +64,9 @@ def print_data(values): Current Voltage --------- %(voltage).1fv Current Load ------------ %(load)d Moving ------------------ %(moving)s -''' %values +''' %values) else: - print '''\ + print('''\ Motor %(id)d is connected: Freespin: False Model ------------------- %(model)s @@ -78,7 +78,7 @@ def print_data(values): Current Voltage --------- %(voltage).1fv Current Load ------------ %(load)d Moving ------------------ %(moving)s -''' %values +''' %values) if __name__ == '__main__': usage_msg = 'Usage: %prog [options] IDs' @@ -104,13 +104,13 @@ def print_data(values): try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError, soe: - print 'ERROR:', soe + print('ERROR:', soe) else: responses = 0 - print 'Pinging motors:' + print('Pinging motors:') for motor_id in motor_ids: motor_id = int(motor_id) - print '%d ...' % motor_id, + print('%d ...' % motor_id, end=' ') p = dxl_io.ping(motor_id) if p: responses += 1 @@ -124,14 +124,14 @@ def print_data(values): values['max'] = angles['max'] values['voltage'] = values['voltage'] values['moving'] = str(values['moving']) - print 'done' + print('done') if angles['max'] == 0 and angles['min'] == 0: values['freespin'] = True else: values['freespin'] = False print_data(values) else: - print 'error' + print('error') if responses == 0: - print 'ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.' + print('ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.') diff --git a/dynamixel_driver/scripts/set_servo_config.py b/dynamixel_driver/scripts/set_servo_config.py index 65b6269..4985e18 100755 --- a/dynamixel_driver/scripts/set_servo_config.py +++ b/dynamixel_driver/scripts/set_servo_config.py @@ -74,7 +74,7 @@ help='set servo motor maximum voltage limit') (options, args) = parser.parse_args(sys.argv) - print options + print(options) if len(args) < 2: parser.print_help() @@ -87,58 +87,58 @@ try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError, soe: - print 'ERROR:', soe + print('ERROR:', soe) else: for motor_id in motor_ids: motor_id = int(motor_id) - print 'Configuring Dynamixel motor with ID %d' % motor_id + print('Configuring Dynamixel motor with ID %d' % motor_id) if dxl_io.ping(motor_id): # check if baud rate needs to be changed if options.baud_rate: valid_rates = (1,3,4,7,9,16,34,103,207,250,251,252) if options.baud_rate not in valid_rates: - print 'Requested baud rate is invalid, please use one of the following: %s' % str(valid_rates) + print('Requested baud rate is invalid, please use one of the following: %s' % str(valid_rates)) if options.baud_rate <= 207: - print 'Setting baud rate to %d bps' % int(2000000.0/(options.baud_rate + 1)) + print('Setting baud rate to %d bps' % int(2000000.0/(options.baud_rate + 1))) elif options.baud_rate == 250: - print 'Setting baud rate to %d bps' % 2250000 + print('Setting baud rate to %d bps' % 2250000) elif options.baud_rate == 251: - print 'Setting baud rate to %d bps' % 2500000 + print('Setting baud rate to %d bps' % 2500000) elif options.baud_rate == 252: - print 'Setting baud rate to %d bps' % 3000000 + print('Setting baud rate to %d bps' % 3000000) dxl_io.set_baud_rate(motor_id, options.baud_rate) # check if return delay time needs to be changed if options.return_delay is not None: if options.return_delay < 0 or options.return_delay > 254: - print 'Requested return delay time is out of valid range (0 - 254)' + print('Requested return delay time is out of valid range (0 - 254)') else: - print 'Setting return delay time to %d us' % (options.return_delay * 2) + print('Setting return delay time to %d us' % (options.return_delay * 2)) dxl_io.set_return_delay_time(motor_id, options.return_delay) # check if CW angle limit needs to be changed if options.cw_angle_limit is not None: - print 'Setting CW angle limit to %d' % options.cw_angle_limit + print('Setting CW angle limit to %d' % options.cw_angle_limit) dxl_io.set_angle_limit_cw(motor_id, options.cw_angle_limit) # check if CCW angle limit needs to be changed if options.ccw_angle_limit is not None: - print 'Setting CCW angle limit to %d' % options.ccw_angle_limit + print('Setting CCW angle limit to %d' % options.ccw_angle_limit) dxl_io.set_angle_limit_ccw(motor_id, options.ccw_angle_limit) # check if minimum voltage limit needs to be changed if options.min_voltage_limit: - print 'Setting minimum voltage limit to %d' % options.min_voltage_limit + print('Setting minimum voltage limit to %d' % options.min_voltage_limit) dxl_io.set_voltage_limit_min(motor_id, options.min_voltage_limit) # check if maximum voltage limit needs to be changed if options.max_voltage_limit: - print 'Setting maximum voltage limit to %d' % options.max_voltage_limit + print('Setting maximum voltage limit to %d' % options.max_voltage_limit) dxl_io.set_voltage_limit_max(motor_id, options.max_voltage_limit) - print 'done' + print('done') else: - print 'Unable to connect to Dynamixel motor with ID %d' % motor_id + print('Unable to connect to Dynamixel motor with ID %d' % motor_id) diff --git a/dynamixel_driver/scripts/set_torque.py b/dynamixel_driver/scripts/set_torque.py index da48e95..f57d31a 100755 --- a/dynamixel_driver/scripts/set_torque.py +++ b/dynamixel_driver/scripts/set_torque.py @@ -75,9 +75,9 @@ try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError, soe: - print 'ERROR:', soe + print('ERROR:', soe) else: - print 'Turning torque %s for motor %d' % (torque_on, motor_id) + print('Turning torque %s for motor %d' % (torque_on, motor_id)) if dxl_io.ping(motor_id): if torque_on.lower() == 'off': torque_on = False @@ -87,7 +87,7 @@ parser.print_help() exit(1) dxl_io.set_torque_enabled(motor_id, torque_on) - print 'done' + print('done') else: - print 'ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id + print('ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id) From 19e5cf61d6a96d6d6c1c8887333b41fed0931b6e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 09:30:07 +0900 Subject: [PATCH 3/4] run 2to3 -w -fexcept . --- dynamixel_controllers/nodes/controller_manager.py | 6 +++--- dynamixel_controllers/nodes/controller_spawner.py | 6 +++--- dynamixel_driver/scripts/set_servo_config.py | 2 +- dynamixel_driver/scripts/set_torque.py | 2 +- .../src/dynamixel_driver/dynamixel_io.py | 4 ++-- .../src/dynamixel_driver/dynamixel_serial_proxy.py | 12 ++++++------ 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/dynamixel_controllers/nodes/controller_manager.py b/dynamixel_controllers/nodes/controller_manager.py index 52c293b..6133ef0 100755 --- a/dynamixel_controllers/nodes/controller_manager.py +++ b/dynamixel_controllers/nodes/controller_manager.py @@ -207,13 +207,13 @@ def start_controller(self, req): # reload module if previously imported package_module = reload(sys.modules[package_path]) controller_module = getattr(package_module, module_name) - except ImportError, ie: + except ImportError as ie: self.start_controller_lock.release() return StartControllerResponse(False, 'Cannot find controller module. Unable to start controller %s\n%s' % (module_name, str(ie))) - except SyntaxError, se: + except SyntaxError as se: self.start_controller_lock.release() return StartControllerResponse(False, 'Syntax error in controller module. Unable to start controller %s\n%s' % (module_name, str(se))) - except Exception, e: + except Exception as e: self.start_controller_lock.release() return StartControllerResponse(False, 'Unknown error has occured. Unable to start controller %s\n%s' % (module_name, str(e))) diff --git a/dynamixel_controllers/nodes/controller_spawner.py b/dynamixel_controllers/nodes/controller_spawner.py index 8efd6af..24351db 100755 --- a/dynamixel_controllers/nodes/controller_spawner.py +++ b/dynamixel_controllers/nodes/controller_spawner.py @@ -73,21 +73,21 @@ def manage_controller(controller_name, port_namespace, controller_type, command, response = start(port_namespace, package_path, module_name, class_name, controller_name, deps) if response.success: rospy.loginfo(response.reason) else: rospy.logerr(response.reason) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.logerr('Service call failed: %s' % e) elif command.lower() == 'stop': try: response = stop(controller_name) if response.success: rospy.loginfo(response.reason) else: rospy.logerr(response.reason) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.logerr('Service call failed: %s' % e) elif command.lower() == 'restart': try: response = restart(port_namespace, package_path, module_name, class_name, controller_name, deps) if response.success: rospy.loginfo(response.reason) else: rospy.logerr(response.reason) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.logerr('Service call failed: %s' % e) else: rospy.logerr('Invalid command.') diff --git a/dynamixel_driver/scripts/set_servo_config.py b/dynamixel_driver/scripts/set_servo_config.py index 4985e18..fc0e6c9 100755 --- a/dynamixel_driver/scripts/set_servo_config.py +++ b/dynamixel_driver/scripts/set_servo_config.py @@ -86,7 +86,7 @@ try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) - except dynamixel_io.SerialOpenError, soe: + except dynamixel_io.SerialOpenError as soe: print('ERROR:', soe) else: for motor_id in motor_ids: diff --git a/dynamixel_driver/scripts/set_torque.py b/dynamixel_driver/scripts/set_torque.py index f57d31a..aa8f108 100755 --- a/dynamixel_driver/scripts/set_torque.py +++ b/dynamixel_driver/scripts/set_torque.py @@ -74,7 +74,7 @@ try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) - except dynamixel_io.SerialOpenError, soe: + except dynamixel_io.SerialOpenError as soe: print('ERROR:', soe) else: print('Turning torque %s for motor %d' % (torque_on, motor_id)) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index c791bdd..ee628ac 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -98,7 +98,7 @@ def __read_response(self, servo_id): if not data[0:2] == ['\xff', '\xff']: raise Exception('Wrong packet prefix %s' % data[0:2]) data.extend(self.ser.read(ord(data[3]))) data = array('B', ''.join(data)).tolist() # [int(b2a_hex(byte), 16) for byte in data] - except Exception, e: + except Exception as e: raise DroppedPacketError('Invalid response received from motor %d. %s' % (servo_id, e)) # verify checksum @@ -241,7 +241,7 @@ def ping(self, servo_id): try: response = self.__read_response(servo_id) response.append(timestamp) - except Exception, e: + except Exception as e: response = [] if response: diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index fcd5867..6b1f175 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -98,7 +98,7 @@ def connect(self): try: self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo) self.__find_motors() - except dynamixel_io.SerialOpenError, e: + except dynamixel_io.SerialOpenError as e: rospy.logfatal(e.message) sys.exit(1) @@ -225,18 +225,18 @@ def __update_motor_states(self): if state: motor_states.append(MotorState(**state)) if dynamixel_io.exception: raise dynamixel_io.exception - except dynamixel_io.FatalErrorCodeError, fece: + except dynamixel_io.FatalErrorCodeError as fece: rospy.logerr(fece) - except dynamixel_io.NonfatalErrorCodeError, nfece: + except dynamixel_io.NonfatalErrorCodeError as nfece: self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) - except dynamixel_io.ChecksumError, cse: + except dynamixel_io.ChecksumError as cse: self.error_counts['checksum'] += 1 rospy.logdebug(cse) - except dynamixel_io.DroppedPacketError, dpe: + except dynamixel_io.DroppedPacketError as dpe: self.error_counts['dropped'] += 1 rospy.logdebug(dpe.message) - except OSError, ose: + except OSError as ose: if ose.errno != errno.EAGAIN: rospy.logfatal(errno.errorcode[ose.errno]) rospy.signal_shutdown(errno.errorcode[ose.errno]) From 0dc32ecc31ca5cd8d05cb81ed708d2f154905a4e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 18 Apr 2023 09:31:15 +0900 Subject: [PATCH 4/4] run 2to3 -w -fimport . --- dynamixel_driver/src/dynamixel_driver/dynamixel_io.py | 2 +- dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index ee628ac..5a6f543 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -48,7 +48,7 @@ from binascii import b2a_hex from threading import Lock -from dynamixel_const import * +from .dynamixel_const import * exception = None diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index 6b1f175..51dcc87 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -53,7 +53,7 @@ roslib.load_manifest('dynamixel_driver') import rospy -import dynamixel_io +from . import dynamixel_io from dynamixel_driver.dynamixel_const import * from diagnostic_msgs.msg import DiagnosticArray