Progress towards python3 compatibility

uuv_manipulators_control/scripts/gripper_controller.py

@@ -13,6 +13,7 @@
 # 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.
+from __future__ import print_function
 import os
 import rospy
 from uuv_manipulator_interfaces import GripperInterface
@@ -138,9 +139,10 @@ def _joy_callback(self, joy):
                 self._ratio_goal = 0.0
             if self._ratio_goal > 1:
                 self._ratio_goal = 1.0
-        except Exception, e:
-            print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
-                  'being used. message=%s' % str(e)
+        except Exception as e:
+            print('Error occurred while parsing joystick input, check if the '
+                  'joy_id corresponds to the joystick being used. '
+                  'message={}'.format(str(e)))
 
 
 if __name__ == '__main__':

uuv_manipulators_control/scripts/joint_position_controller.py

@@ -13,6 +13,7 @@
 # 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.
+from __future__ import print_function
 import os
 import rospy
 from copy import deepcopy
@@ -132,8 +133,8 @@ def _joy_callback(self, joy):
                         # Check for the joint limits
                         self._reference_pos[joint] = self._check_joint_limits(self._reference_pos[joint], joint)
                 self._last_joy_update = rospy.get_time()
-        except Exception, e:
-            print 'Error during joy parsing, message=', e
+        except Exception as e:
+            print('Error during joy parsing, message={}'.format(e))
 
 if __name__ == '__main__':
     # Start the node

uuv_manipulators_control/scripts/set_joint_config.py

@@ -13,7 +13,7 @@
 # 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.
-
+from __future__ import print_function
 import argparse
 import sys
 import rospy
@@ -59,11 +59,11 @@
 
         joint_pos = rospy.get_param(namespace + 'arms/' + arm_name + '/default_configs/' + config)
 
-        print 'Set joint configuration=', joint_pos
-        print 'Output topics:'
+        print('Set joint configuration={}'.format(joint_pos))
+        print('Output topics:')
         pub = {}
         for joint in joint_pos:
-            print '     ', joint, '=', namespace + arm_name + '/' + joint + '/position_controller/command'
+            print('     {}={}/{}/position_controller/command'.format(joint, namespace + arm_name, joint))
             pub[joint] = rospy.Publisher(namespace + arm_name + '/' + joint + '/position_controller/command', Float64, queue_size=1)
 
         start_time = time.clock()
@@ -72,7 +72,7 @@
         rospy.loginfo('Publishing command message for %s seconds' % duration)
         while time.clock() <= start_time + duration:
             if rospy.is_shutdown():
-                print 'ROS is not running'
+                print('ROS is not running')
                 break
 
             for joint in joint_pos:
@@ -84,5 +84,5 @@
         rospy.loginfo('Finishing setting joint configuration')
 
     except rospy.ROSInterruptException:
-        print 'uuv_manipulators_control::set_joint_config::Exception'
-    print 'Leaving uuv_manipulators_control::set_joint_config'
+        print('uuv_manipulators_control::set_joint_config::Exception')
+    print('Leaving uuv_manipulators_control::set_joint_config')

uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py

@@ -12,7 +12,7 @@
 # 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.
-
+from __future__ import print_function
 import rospy
 from uuv_manipulator_interfaces import ArmInterface
 from geometry_msgs.msg import Twist, PoseStamped, Quaternion, Vector3
@@ -139,7 +139,7 @@ def _get_goal(self):
         if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None:
             return next_goal
         else:
-            print 'Next goal could not be resolved by the inv. kinematics solver.'
+            print('Next goal could not be resolved by the inv. kinematics solver.')
             return self._last_goal
 
     def _home_button_pressed(self, msg):

uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py

@@ -12,6 +12,9 @@
 # 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.
+from __future__ import print_function
+from past.builtins import xrange
+from future.utils import listvalues
 
 from copy import deepcopy
 import rospy
@@ -168,7 +171,7 @@ def update_endeffector_state(self):
         # End effector pose
         pose = self.forward_position_kinematics(q)
         vel = self.forward_velocity_kinematics(q, qd)
-        wrench = np.dot(self.jacobian(q), np.array(eff.values()))
+        wrench = np.dot(self.jacobian(q), np.array(listvalues(eff)))
         # Store everything in the end point state message
         self._endeffector_state.position = pose[0:3]
         self._endeffector_state.orientation = pose[3::]
@@ -228,7 +231,7 @@ def get_config(self, config='home'):
 
     def add_callback(self, topic_name, function_handle):
         if topic_name not in self._subTopics:
-            print 'ArmInterface - Invalid topic name'
+            print('ArmInterface - Invalid topic name')
             return
 
         if topic_name not in self._callbacks:

uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py

@@ -12,7 +12,8 @@
 # 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.
-
+from __future__ import print_function
+from past.builtins import xrange
 import rospy
 from copy import deepcopy
 import numpy as np
@@ -230,18 +231,18 @@ def print_robot_description(self):
         for j in self._robot_description.joints:
             if j.type != 'fixed':
                 nf_joints += 1
-        print 'Base root=%s' % self._base_link
-        print 'Tip link=%s' % self._tip_link
-        print "URDF non-fixed joints: %d;" % nf_joints
-        print "URDF total joints: %d" % len(self.n_joints)
-        print "URDF links: %d" % len(self._robot_description.links)
-        print "KDL joints: %d" % self._kdl_tree.getNrOfJoints()
-        print "KDL segments: %d" % self._kdl_tree.getNrOfSegments()
+        print('Base root=%s' % self._base_link)
+        print('Tip link=%s' % self._tip_link)
+        print("URDF non-fixed joints: %d;" % nf_joints)
+        print("URDF total joints: %d" % len(self.n_joints))
+        print("URDF links: %d" % len(self._robot_description.links))
+        print("KDL joints: %d" % self._kdl_tree.getNrOfJoints())
+        print("KDL segments: %d" % self._kdl_tree.getNrOfSegments())
 
     def print_chain(self):
-        print 'Number of segments in chain=%d' % self._chain.getNrOfSegments()
+        print('Number of segments in chain=%d' % self._chain.getNrOfSegments())
         for idx in xrange(self._chain.getNrOfSegments()):
-            print '* ' + self._chain.getSegment(idx).getName()
+            print('* ' + self._chain.getSegment(idx).getName())
 
     def get_joint_angle(self, joint):
         assert joint in self._joint_angles, 'Invalid joint name'