Merge 'upstream/master'

Author: ThyMYthOS

README.rst

@@ -12,6 +12,7 @@ Supported ROS Versions
 - Indigo
 - Kinetic
 - Melodic
+- Noetic
 
 Installation
 ------------

intera_examples/CMakeLists.txt

@@ -36,6 +36,37 @@ catkin_package(
   intera_interface
 )
 
+catkin_install_python(PROGRAMS 
+  scripts/ik_service_client.py
+  scripts/go_to_joint_angles.py
+  scripts/joint_position_waypoints.py
+  scripts/joint_torque_springs.py
+  scripts/gripper_cuff_control.py
+  scripts/head_wobbler.py
+  scripts/joint_recorder.py
+  scripts/fk_service_client.py
+  scripts/joint_velocity_wobbler.py
+  scripts/navigator_io.py
+  scripts/joint_position_joystick.py
+  scripts/joint_position_keyboard.py
+  scripts/gripper_keyboard.py
+  scripts/stop_motion_trajectory.py
+  scripts/go_to_joint_angles_in_contact.py
+  scripts/joint_position_file_playback.py
+  scripts/go_to_cartesian_pose.py
+  scripts/head_display_image.py
+  scripts/joint_trajectory_client.py
+  scripts/joint_trajectory_file_playback.py
+  scripts/set_interaction_options.py
+  scripts/constrained_zeroG.py
+  scripts/gripper_joystick.py
+  scripts/lights_blink.py
+  scripts/camera_display.py
+  scripts/send_random_trajectory.py
+  scripts/send_traj_from_file.py
+
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
 install(
   DIRECTORY scripts/
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

intera_examples/cfg/SawyerJointSpringsExample.cfg

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>intera_examples</name>
   <version>5.3.0</version>
   <description>
@@ -21,6 +21,8 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
   <build_depend>rospy</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -32,16 +34,16 @@
   <build_depend>intera_motion_msgs</build_depend>
   <build_depend>intera_interface</build_depend>
 
-  <run_depend>rospy</run_depend>
-  <run_depend>actionlib</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>control_msgs</run_depend>
-  <run_depend>trajectory_msgs</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>intera_core_msgs</run_depend>
-  <run_depend>intera_motion_msgs</run_depend>
-  <run_depend>intera_interface</run_depend>
-  <run_depend>joystick_drivers</run_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>intera_core_msgs</exec_depend>
+  <exec_depend>intera_motion_msgs</exec_depend>
+  <exec_depend>intera_interface</exec_depend>
+  <exec_depend>joystick_drivers</exec_depend>
 
 </package>

intera_examples/scripts/camera_display.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -23,13 +22,14 @@
 import rospy
 import intera_interface
 
-def show_image_callback(img_data, (edge_detection, window_name)):
+def show_image_callback(img_data, xxx_todo_changeme):
     """The callback function to show image by using CvBridge and cv
     """
+    (edge_detection, window_name) = xxx_todo_changeme
     bridge = CvBridge()
     try:
         cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
-    except CvBridgeError, err:
+    except CvBridgeError as err:
         rospy.logerr(err)
         return
     if edge_detection == True:

intera_examples/scripts/constrained_zeroG.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python
-
 # Copyright (c) 2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/fk_service_client.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -49,7 +48,7 @@ def fk_service_client(limb = "right"):
     try:
         rospy.wait_for_service(ns, 5.0)
         resp = fksvc(fkreq)
-    except (rospy.ServiceException, rospy.ROSException), e:
+    except (rospy.ServiceException, rospy.ROSException) as e:
         rospy.logerr("Service call failed: %s" % (e,))
         return False
 

intera_examples/scripts/go_to_cartesian_pose.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python
-
 # Copyright (c) 2016-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/go_to_joint_angles.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python
-
 # Copyright (c) 2016-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/go_to_joint_angles_in_contact.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python
-
 # Copyright (c) 2016-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/gripper_cuff_control.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/gripper_joystick.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/gripper_keyboard.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -95,7 +94,7 @@ def update_velocity(offset_vel):
                 print("key bindings: ")
                 print("  Esc: Quit")
                 print("  ?: Help")
-                for key, val in sorted(bindings.items(),
+                for key, val in sorted(list(bindings.items()),
                                        key=lambda x: x[1][2]):
                     print("  %s: %s" % (key, val[2]))
     # force shutdown call if caught by key handler

intera_examples/scripts/head_display_image.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/head_wobbler.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/ik_service_client.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -90,7 +89,7 @@ def ik_service_client(limb = "right", use_advanced_options = False):
     try:
         rospy.wait_for_service(ns, 5.0)
         resp = iksvc(ikreq)
-    except (rospy.ServiceException, rospy.ROSException), e:
+    except (rospy.ServiceException, rospy.ROSException) as e:
         rospy.logerr("Service call failed: %s" % (e,))
         return False
 
@@ -104,7 +103,7 @@ def ik_service_client(limb = "right", use_advanced_options = False):
         rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
               (seed_str,))
         # Format solution into Limb API-compatible dictionary
-        limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
+        limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
         rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
         rospy.loginfo("------------------")
         rospy.loginfo("Response Message:\n%s", resp)

intera_examples/scripts/joint_position_file_playback.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -40,12 +39,12 @@ def clean_line(line, names):
     #convert the line of strings to a float or None
     line = [try_float(x) for x in line.rstrip().split(',')]
     #zip the values with the joint names
-    combined = zip(names[1:], line[1:])
+    combined = list(zip(names[1:], line[1:]))
     #take out any tuples that have a none value
     cleaned = [x for x in combined if x[1] is not None]
     #convert it to a dictionary with only valid commands
     command = dict(cleaned)
-    right_command = dict((key, command[key]) for key in command.keys()
+    right_command = dict((key, command[key]) for key in list(command.keys())
                          if key[:-2] == 'right_')
     return (command, right_command, line)
 
@@ -64,10 +63,10 @@ def map_file(filename, limb, loops=1):
     first column is the time stamp
     """
     limb_interface = intera_interface.Limb(limb)
-    has_gripper = True
+    has_gripper = False
     try:
         gripper = intera_interface.Gripper(limb + '_gripper')
-    except ValueError:
+    except:
         has_gripper = False
         rospy.loginfo("Could not detect a gripper attached to the robot")
 
@@ -109,7 +108,7 @@ def map_file(filename, limb, loops=1):
                 if has_gripper and gripper.name in cmd:
                         gripper.set_position(cmd[gripper.name])
                 rate.sleep()
-        print
+        print()
     return True
 
 def main():

intera_examples/scripts/joint_position_joystick.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/joint_position_keyboard.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -98,7 +97,7 @@ def set_g(action):
                 print("key bindings: ")
                 print("  Esc: Quit")
                 print("  ?: Help")
-                for key, val in sorted(bindings.items(),
+                for key, val in sorted(list(bindings.items()),
                                        key=lambda x: x[1][2]):
                     print("  %s: %s" % (key, val[2]))
 

intera_examples/scripts/joint_position_waypoints.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/joint_recorder.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/joint_torque_springs.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#! /usr/bin/env python
 
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
@@ -89,7 +89,7 @@ def _update_forces(self):
         cur_pos = self._limb.joint_angles()
         cur_vel = self._limb.joint_velocities()
         # calculate current forces
-        for joint in self._start_angles.keys():
+        for joint in list(self._start_angles.keys()):
             # spring portion
             cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
                                                    cur_pos[joint])

intera_examples/scripts/joint_trajectory_client.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");

intera_examples/scripts/joint_trajectory_file_playback.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -135,7 +134,7 @@ def try_float(x):
         #convert the line of strings to a float or None
         line = [try_float(x) for x in line.rstrip().split(',')]
         #zip the values with the joint names
-        combined = zip(joint_names[1:], line[1:])
+        combined = list(zip(joint_names[1:], line[1:]))
         #take out any tuples that have a none value
         cleaned = [x for x in combined if x[1] is not None]
         #convert it to a dictionary with only valid commands
@@ -189,10 +188,10 @@ def find_start_offset(pos):
                     cur.append(self.arm.joint_angle(name))
                     prm = rospy.get_param(vel_param % name, 0.25)
                     dflt_vel.append(prm)
-            diffs = map(operator.sub, cmd, cur)
-            diffs = map(operator.abs, diffs)
+            diffs = list(map(operator.sub, cmd, cur))
+            diffs = list(map(operator.abs, diffs))
             #determine the largest time offset necessary across all joints
-            offset = max(map(operator.div, diffs, dflt_vel))
+            offset = max(list(map(operator.div, diffs, dflt_vel)))
             return offset
 
         for idx, values in enumerate(lines[1:]):

intera_examples/scripts/joint_velocity_wobbler.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
@@ -54,7 +53,7 @@ def __init__(self):
 
     def _reset_control_modes(self):
         rate = rospy.Rate(self._rate)
-        for _ in xrange(100):
+        for _ in range(100):
             if rospy.is_shutdown():
                 return False
             self._right_arm.exit_control_mode()
@@ -105,7 +104,7 @@ def make_cmd(joint_names, elapsed):
 
         print("Wobbling. Press Ctrl-C to stop...")
         while not rospy.is_shutdown():
-            self._pub_rate.publish(self._rate)
+            self._pub_rate.publish(int(self._rate))
             elapsed = rospy.Time.now() - start
             cmd = make_cmd(self._right_joint_names, elapsed)
             self._right_arm.set_joint_velocities(cmd)

intera_examples/scripts/lights_blink.py

@@ -1,5 +1,4 @@
-#!/usr/bin/env python
-
+#! /usr/bin/env python
 # Copyright (c) 2013-2018, Rethink Robotics Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");