From c22efa93794c0a52d5d8e64406c62e38d1a86889 Mon Sep 17 00:00:00 2001
From: Mikhail Medvedev <mmedvede@cs.uml.edu>
Date: Sat, 26 Oct 2013 22:17:47 -0400
Subject: [PATCH] adds visualization class

---
 manifest.xml        |  2 ++
 src/vis/__init__.py |  1 +
 src/vis/vis.py      | 55 +++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 58 insertions(+)
 create mode 100644 src/vis/__init__.py
 create mode 100644 src/vis/vis.py

diff --git a/manifest.xml b/manifest.xml
index a5790aa..b202944 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -13,6 +13,8 @@
   <depend package="baxter_interface"/>
   <depend package="utilities"/>
   <depend package="baxter_faces"/>
+  <depend package="visualization_msgs"/>
+
 
 </package>
 
diff --git a/src/vis/__init__.py b/src/vis/__init__.py
new file mode 100644
index 0000000..d785fdf
--- /dev/null
+++ b/src/vis/__init__.py
@@ -0,0 +1 @@
+from vis import Vis
diff --git a/src/vis/vis.py b/src/vis/vis.py
new file mode 100644
index 0000000..31389cc
--- /dev/null
+++ b/src/vis/vis.py
@@ -0,0 +1,55 @@
+import roslib
+roslib.load_manifest('baxter_hydra_teleop')
+import rospy
+
+from std_msgs.msg import Header
+
+from visualization_msgs.msg import Marker
+
+
+class Vis:
+    def __init__(self):
+        self.pub = rospy.Publisher(
+            '/visualization_marker', Marker)
+        self.last_time = rospy.Time.now()
+
+    def show_gripper(self, limb):
+        # Throttle
+        if (rospy.Time.now() - self.last_time) < rospy.Duration(0.05):
+            return
+        self.last_time = rospy.Time.now()
+
+        hdr = Header(
+            #stamp=rospy.Time.now(), frame_id='left_gripper')
+            stamp=rospy.Time.now(), frame_id='hydra_' + limb + '_grab')
+
+        # Gripper projection
+        msg = Marker(header=hdr, ns=limb)
+        msg.scale.x = 0.009
+        msg.scale.y = 0.009
+        msg.scale.z = 0.11
+        msg.type = Marker.CYLINDER
+        msg.color.b = 0.5
+        msg.color.a = 0.8
+        msg.pose.position.z = -0.055
+        msg.pose.position.y = -0.025
+        msg.pose.orientation.x = 1
+        self.pub.publish(msg)
+        msg.id += 1
+        msg.pose.position.y = +0.025
+        self.pub.publish(msg)
+
+        # Hydra
+        msg.id += 1
+        msg.scale.x = 0.06
+        msg.scale.y = 0.04
+        msg.scale.z = 0.18
+        msg.pose.position.x = -0.09
+        msg.pose.position.y = 0
+        msg.pose.position.z = 0
+        msg.pose.orientation.z = 1
+        msg.color.r = 0.2
+        msg.color.g = 0.2
+        msg.color.b = 0.2
+        msg.color.a = 0.2
+        self.pub.publish(msg)