Move classes out of teleop.py

scripts/teleop.py

@@ -33,223 +33,17 @@
 
 """
 
-import sys
 import threading
 
 import roslib
 roslib.load_manifest('baxter_hydra_teleop')
 import rospy
-import tf
 
 from razer_hydra.msg import Hydra
-
-from geometry_msgs.msg import (
-    PoseStamped,
-    Pose,
-    Point,
-    Quaternion,
-    Transform,
-)
-from std_msgs.msg import Header
-from baxter_msgs.srv import SolvePositionIK
-from baxter_msgs.srv import SolvePositionIKRequest
 import baxter_interface
-
 import baxter_faces
-
-from baxter_hydra_teleop import vis
-
-
-class HeadMover(object):
-    def __init__(self):
-        self._head = baxter_interface.Head()
-        self.pan_angle = 0
-
-    def set_pose(self):
-        self._head.set_pan(self.pan_angle)
-
-    def parse_joy(self, joypad):
-        if joypad.joy[0] != 0:
-            increment = -joypad.joy[0] / 50
-            self.pan_angle += increment
-            if abs(self.pan_angle) > 1.57:
-                self.pan_angle -= increment
-            self.set_pose()
-
-
-class LimbMover(object):
-    def __init__(self, limb):
-        self.limb = limb
-        self.interface = baxter_interface.Limb(limb)
-        self.solver = IKSolver(limb)
-        self.last_solve_request_time = rospy.Time.now()
-        self.running = True
-        self.thread = threading.Thread(target=self._update_thread)
-        self.vis = vis.Vis()
-        self.goal_transform = GoalTransform(limb)
-
-    def enable(self):
-        self.thread.start()
-
-    def set_target(self, joints):
-        self.target_joints = joints
-
-    def _update_req_time(self):
-        self.last_solve_request_time = rospy.Time.now()
-
-    def _solver_cooled_down(self):
-        time_since_req = rospy.Time.now() - self.last_solve_request_time
-        return time_since_req > rospy.Duration(0.05)  # 20 Hz
-
-    def update(self, trigger, gripper_travel):
-        self.vis.show_gripper(self.limb, gripper_travel, 0.026, 0.11, 1)
-        self.goal_transform.update()
-
-        # Throttle service requests
-        if trigger and self._solver_cooled_down():
-            self._update_req_time()
-            return self.solver.solve()
-        return True
-
-    def stop_thread(self):
-        if self.thread.is_alive():
-            self.running = False
-            self.thread.join()
-
-    def _update_thread(self):
-        rospy.loginfo("Starting Joint Update Thread: %s\n" % self.limb)
-        rate = rospy.Rate(200)
-        while not rospy.is_shutdown() and self.running:
-            self.interface.set_joint_positions(self.solver.solution)
-            rate.sleep()
-        rospy.loginfo("Stopped %s" % self.limb)
-
-
-class IKSolver(object):
-    def __init__(self, limb):
-        self.limb = limb
-        ns = "/sdk/robot/limb/" + self.limb + "/solve_ik_position"
-        rospy.wait_for_service(ns)
-        self.iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
-        self.solution = dict()
-        self.mapping = [
-            # Human-like mapping (front of camera  = front of the wrist)
-            Quaternion(
-                x=0,
-                y=0.7071067811865475244,  # sqrt(0.5)
-                z=0,
-                w=0.7071067811865475244  # sqrt(0.5)
-            # Camera is pointing down
-            ), Quaternion(
-                x=0,
-                y=1,
-                z=0,
-                w=0
-            ),
-        ]
-
-    def solve(self):
-        ikreq = SolvePositionIKRequest()
-        hdr = Header(
-            stamp=rospy.Time.now(), frame_id=self.limb + '_gripper_goal')
-        pose = PoseStamped(
-            header=hdr,
-            pose=Pose(
-                position=Point(x=0, y=0, z=0),
-                orientation=self.mapping[1]
-            ),
-        )
-
-        ikreq.pose_stamp.append(pose)
-        try:
-            resp = self.iksvc(ikreq)
-        except rospy.ServiceException, e:
-            rospy.loginfo("Service call failed: %s" % (e,))
-
-        if (resp.isValid[0]):
-            self.solution = dict(
-                zip(resp.joints[0].names, resp.joints[0].angles))
-            rospy.loginfo("Solution Found, %s" % self.limb, self.solution)
-            return True
-
-        else:
-            rospy.logwarn("INVALID POSE for %s" % self.limb)
-            return False
-
-
-class GoalTransform(object):
-    """ Publish an additional transforms constrained in some way.
-
-    Should allow to make it easier to add teleoperation constraints,
-    e.g. lock the control plane, or change motion scaling.
-    """
-
-    def __init__(self, limb):
-        self.modes = {
-                      "identity": self._identity,
-                      "orientation": self._orientation_lock,
-                      "plane": self._plane_lock
-                      }
-        self.set_mode("identity")
-        self.br = tf.TransformBroadcaster()
-        self.plane = Transform()
-        self.plane.rotation.w = 1
-        self.limb = limb
-        self.orientation = Quaternion(0, 0, 0, 1)
-        self.orientation_lock_frame = "base"
-        self.tf_listener = tf.TransformListener()
-        """
-        _tf_offset is used to add an offset to the stamp of published
-        transforms. It is a hack needed to IK nodes on Baxter happy, and
-        would be different for different systems.
-        """
-        self._tf_offset = 0.1
-
-        self.plane = Pose()
-
-    def set_mode(self, mode):
-        self.updater = self.modes[mode]
-
-    def update(self):
-        self.updater()
-
-    def _identity(self):
-        """ No tranformation, 1 to 1 mapping """
-        self.br.sendTransform(
-           (0, 0, 0),
-           (0, 0, 0, 1),
-           rospy.Time.now() + rospy.Duration(self._tf_offset),
-            self.limb + "_gripper_goal",
-           "hydra_" + self.limb + "_grab")
-
-    def _orientation_lock(self):
-        """ Lock the orientation """
-        try:
-            (trans, rot) = self.tf_listener.lookupTransform(
-                   self.orientation_lock_frame,
-                   'hydra_' + self.limb + '_grab',
-                   rospy.Time(0))
-        except (tf.LookupException,
-                tf.ConnectivityException,
-                tf.ExtrapolationException) as e:
-            print e
-            return
-
-        self.br.sendTransform(
-           trans,
-           (
-            self.orientation.x,
-            self.orientation.y,
-            self.orientation.z,
-            self.orientation.w,
-           ),
-           rospy.Time.now() + rospy.Duration(self._tf_offset),
-           self.limb + "_gripper_goal",
-           self.orientation_lock_frame)
-
-    def _plane_lock(self):
-        """ Lock the orientation """
-        pass
+from baxter_hydra_teleop.head_mover import HeadMover
+from baxter_hydra_teleop.limb_mover import LimbMover
 
 
 class Teleop(object):
@@ -271,7 +65,7 @@ def __init__(self):
         self.hydra_msg = Hydra()
 
         rospy.on_shutdown(self._cleanup)
-        sub = rospy.Subscriber("/hydra_calib", Hydra, self._hydra_cb)
+        rospy.Subscriber("/hydra_calib", Hydra, self._hydra_cb)
 
         rospy.Timer(rospy.Duration(1.0 / 30), self._main_loop)
 

src/baxter_hydra_teleop/goal_transform.py

@@ -0,0 +1,112 @@
+# Copyright (c) 2013, University Of Massachusetts Lowell
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the University of Massachusetts Lowell nor the names
+#    from of its contributors may be used to endorse or promote products
+#    derived this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+roslib.load_manifest('baxter_hydra_teleop')
+import rospy
+import tf
+
+from geometry_msgs.msg import (
+    Pose,
+    Transform,
+    Quaternion,
+)
+
+
+class GoalTransform(object):
+    """ Publish an additional transforms constrained in some way.
+
+    Should allow to make it easier to add teleoperation constraints,
+    e.g. lock the control plane, or change motion scaling.
+    """
+
+    def __init__(self, limb):
+        self.modes = {
+                      "identity": self._identity,
+                      "orientation": self._orientation_lock,
+                      "plane": self._plane_lock
+                      }
+        self.set_mode("identity")
+        self.br = tf.TransformBroadcaster()
+        self.plane = Transform()
+        self.plane.rotation.w = 1
+        self.limb = limb
+        self.orientation = Quaternion(0, 0, 0, 1)
+        self.orientation_lock_frame = "base"
+        self.tf_listener = tf.TransformListener()
+        """
+        _tf_offset is used to add an offset to the stamp of published
+        transforms. It is a hack needed to IK nodes on Baxter happy, and
+        would be different for different systems.
+        """
+        self._tf_offset = 0.1
+
+        self.plane = Pose()
+
+    def set_mode(self, mode):
+        self.updater = self.modes[mode]
+
+    def update(self):
+        self.updater()
+
+    def _identity(self):
+        """ No transformation, 1 to 1 mapping """
+        self.br.sendTransform(
+           (0, 0, 0),
+           (0, 0, 0, 1),
+           rospy.Time.now() + rospy.Duration(self._tf_offset),
+            self.limb + "_gripper_goal",
+           "hydra_" + self.limb + "_grab")
+
+    def _orientation_lock(self):
+        """ Lock the orientation """
+        try:
+            (trans, rot) = self.tf_listener.lookupTransform(
+                   self.orientation_lock_frame,
+                   'hydra_' + self.limb + '_grab',
+                   rospy.Time(0))
+        except (tf.LookupException,
+                tf.ConnectivityException,
+                tf.ExtrapolationException) as e:
+            print e
+            return
+
+        self.br.sendTransform(
+           trans,
+           (
+            self.orientation.x,
+            self.orientation.y,
+            self.orientation.z,
+            self.orientation.w,
+           ),
+           rospy.Time.now() + rospy.Duration(self._tf_offset),
+           self.limb + "_gripper_goal",
+           self.orientation_lock_frame)
+
+    def _plane_lock(self):
+        """ Lock the orientation """
+        pass

src/baxter_hydra_teleop/head_mover.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2013, University Of Massachusetts Lowell
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the University of Massachusetts Lowell nor the names
+#    from of its contributors may be used to endorse or promote products
+#    derived this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+roslib.load_manifest('baxter_hydra_teleop')
+
+import baxter_interface
+
+
+class HeadMover(object):
+    def __init__(self):
+        self._head = baxter_interface.Head()
+        self.pan_angle = 0
+
+    def set_pose(self):
+        self._head.set_pan(self.pan_angle)
+
+    def parse_joy(self, joypad):
+        if joypad.joy[0] != 0:
+            increment = -joypad.joy[0] / 50
+            self.pan_angle += increment
+            if abs(self.pan_angle) > 1.57:
+                self.pan_angle -= increment
+            self.set_pose()