Use package format3 use pykdl

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

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.')

dynamixel_driver/package.xml

@@ -1,4 +1,4 @@
-<package>
+<package format="3">
   <name>dynamixel_driver</name>
   <version>0.4.1</version>
   <description>
@@ -22,9 +22,10 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <run_depend>rospy</run_depend>
-  <run_depend>diagnostic_msgs</run_depend>
-  <run_depend>python-serial</run_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-serial</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-serial</exec_depend>
 
   <export>
 

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)

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.')
 

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()
@@ -86,59 +86,59 @@
 
     try:
         dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
-    except dynamixel_io.SerialOpenError, soe:
-        print 'ERROR:', soe
+    except dynamixel_io.SerialOpenError as 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)

dynamixel_driver/scripts/set_torque.py

@@ -74,10 +74,10 @@
 
     try:
         dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
-    except dynamixel_io.SerialOpenError, soe:
-        print 'ERROR:', soe
+    except dynamixel_io.SerialOpenError as 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)
 

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
 
@@ -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:

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
@@ -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])