Support Python3

dynamixel_controllers/nodes/controller_manager.py

@@ -49,6 +49,8 @@
 
 import rospy
 
+from importlib import import_module
+
 from dynamixel_driver.dynamixel_serial_proxy import SerialProxy
 
 from diagnostic_msgs.msg import DiagnosticArray
@@ -78,7 +80,7 @@ def __init__(self):
         manager_namespace = rospy.get_param('~namespace')
         serial_ports = rospy.get_param('~serial_ports')
 
-        for port_namespace,port_config in serial_ports.items():
+        for port_namespace,port_config in list(serial_ports.items()):
             port_name = port_config['port_name']
             baud_rate = port_config['baud_rate']
             readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False
@@ -132,7 +134,7 @@ def __init__(self):
         if self.diagnostics_rate > 0: Thread(target=self.diagnostics_processor).start()
 
     def on_shutdown(self):
-        for serial_proxy in self.serial_proxies.values():
+        for serial_proxy in list(self.serial_proxies.values()):
             serial_proxy.disconnect()
 
     def diagnostics_processor(self):
@@ -143,7 +145,7 @@ def diagnostics_processor(self):
             diag_msg.status = []
             diag_msg.header.stamp = rospy.Time.now()
 
-            for controller in self.controllers.values():
+            for controller in list(self.controllers.values()):
                 try:
                     joint_state = controller.joint_state
                     temps = joint_state.motor_temps
@@ -173,9 +175,9 @@ def check_deps(self):
         controllers_still_waiting = []
 
         for i,(controller_name,deps,kls) in enumerate(self.waiting_meta_controllers):
-            if not set(deps).issubset(self.controllers.keys()):
+            if not set(deps).issubset(list(self.controllers.keys())):
                 controllers_still_waiting.append(self.waiting_meta_controllers[i])
-                rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(self.controllers.keys())))))
+                rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(list(self.controllers.keys()))))))
             else:
                 dependencies = [self.controllers[dep_name] for dep_name in deps]
                 controller = kls(controller_name, dependencies)
@@ -200,20 +202,21 @@ def start_controller(self, req):
             return StartControllerResponse(False, 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name)
 
         try:
-            if module_name not in sys.modules:
-                # import if module not previously imported
-                package_module = __import__(package_path, globals(), locals(), [module_name], -1)
-            else:
-                # reload module if previously imported
-                package_module = reload(sys.modules[package_path])
-            controller_module = getattr(package_module, module_name)
-        except ImportError, ie:
+            # if module_name not in sys.modules:
+            #     # import if module not previously imported
+            #     package_module = import_module(package_path)
+            # else:
+            #     # reload module if previously imported
+            #     package_module = importlib.reload(sys.modules[package_path])
+            # controller_module = getattr(package_module, module_name)
+            controller_module = import_module(package_path+'.'+module_name) # (the temporal modification) always import modules without reload
+        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)))
 
@@ -227,7 +230,7 @@ def start_controller(self, req):
 
         if port_name != 'meta' and (port_name not in self.serial_proxies):
             self.start_controller_lock.release()
-            return StartControllerResponse(False, 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (port_name, str(self.serial_proxies.keys()), controller_name))
+            return StartControllerResponse(False, 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (port_name, str(list(self.serial_proxies.keys())), controller_name))
 
         controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name)
 

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_controllers/src/dynamixel_controllers/joint_position_controller.py

@@ -32,7 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from __future__ import division
 
 
 __author__ = 'Antons Rebguns'
@@ -166,7 +165,7 @@ def set_acceleration_raw(self, acc):
 
     def process_motor_states(self, state_list):
         if self.running:
-            state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
+            state = [state for state in state_list.motor_states if state.id == self.motor_id]
             if state:
                 state = state[0]
                 self.joint_state.motor_temps = [state.temperature]

dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py

@@ -32,7 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from __future__ import division
 
 
 __author__ = 'Antons Rebguns'

dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller.py

@@ -32,7 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from __future__ import division
 
 
 __author__ = 'Antons Rebguns'
@@ -151,7 +150,7 @@ def set_torque_limit(self, max_torque):
 
     def process_motor_states(self, state_list):
         if self.running:
-            state = filter(lambda state: state.id == self.motor_id, state_list.motor_states)
+            state = [state for state in state_list.motor_states if state.id == self.motor_id]
             if state:
                 state = state[0]
                 self.joint_state.motor_temps = [state.temperature]

dynamixel_controllers/src/dynamixel_controllers/joint_torque_controller_dual_motor.py

@@ -32,7 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from __future__ import division
 
 
 __author__ = 'Antons Rebguns'

dynamixel_controllers/src/dynamixel_controllers/joint_trajectory_action_controller.py

@@ -32,7 +32,6 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from __future__ import division
 
 
 __author__ = 'Antons Rebguns'
@@ -85,9 +84,9 @@ def __init__(self, controller_namespace, controllers):
             if c.port_namespace in self.port_to_io: continue
             self.port_to_io[c.port_namespace] = c.dxl_io
 
-        self.joint_states = dict(zip(self.joint_names, [c.joint_state for c in controllers]))
+        self.joint_states = dict(list(zip(self.joint_names, [c.joint_state for c in controllers])))
         self.num_joints = len(self.joint_names)
-        self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
+        self.joint_to_idx = dict(list(zip(self.joint_names, list(range(self.num_joints)))))
 
     def initialize(self):
         ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
@@ -245,7 +244,7 @@ def process_trajectory(self, traj):
 
             multi_packet = {}
 
-            for port, joints in self.port_to_joints.items():
+            for port, joints in list(self.port_to_joints.items()):
                 vals = []
 
                 for joint in joints:
@@ -276,7 +275,7 @@ def process_trajectory(self, traj):
 
                 multi_packet[port] = vals
 
-            for port, vals in multi_packet.items():
+            for port, vals in list(multi_packet.items()):
                 self.port_to_io[port].set_multi_position_and_speed(vals)
 
             while time < seg_end_times[seg]:
@@ -286,7 +285,7 @@ def process_trajectory(self, traj):
                     msg = 'New trajectory received. Aborting old trajectory.'
                     multi_packet = {}
 
-                    for port, joints in self.port_to_joints.items():
+                    for port, joints in list(self.port_to_joints.items()):
                         vals = []
 
                         for joint in joints:
@@ -299,7 +298,7 @@ def process_trajectory(self, traj):
 
                         multi_packet[port] = vals
 
-                    for port, vals in multi_packet.items():
+                    for port, vals in list(multi_packet.items()):
                         self.port_to_io[port].set_multi_position(vals)
 
                     self.action_server.set_preempted(text=msg)
@@ -337,10 +336,10 @@ def process_trajectory(self, traj):
                 self.action_server.set_aborted(result=res, text=msg)
                 break
         else:
-	    msg = 'Trajectory execution successfully completed'
-	    rospy.loginfo(msg)
-	    res = FollowJointTrajectoryResult()  
-	    res.error_code=FollowJointTrajectoryResult.SUCCESSFUL
+            msg = 'Trajectory execution successfully completed'
+            rospy.loginfo(msg)
+            res = FollowJointTrajectoryResult()
+            res.error_code=FollowJointTrajectoryResult.SUCCESSFUL
             self.action_server.set_succeeded(result=res, text=msg)
 
     def update_state(self):

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

@@ -70,17 +70,17 @@
 
     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 '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'
@@ -103,14 +103,14 @@ def print_data(values):
 
     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:
         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
@@ -119,19 +119,19 @@ def print_data(values):
                 model = dxl_io.get_model_number(motor_id)
                 firmware = dxl_io.get_firmware_version(motor_id)
                 values['model'] = '%s (firmware version: %d)' % (DXL_MODEL_TO_PARAMS[model]['name'], firmware)
-                values['degree_symbol'] = u"\u00B0"
+                values['degree_symbol'] = "\u00B0"
                 values['min'] = angles['min']
                 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.')