click_to_if.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('projected_lightswitch_interface')
import rospy
import tf
from geometry_msgs.msg import Point, PointStamped, PolygonStamped
from pr2_object_manipulation_msgs.msg import ImageClick
from std_msgs.msg import ColorRGBA, String
from projector_interface.srv import GetCursorStats, DrawPolygon, CircleInhibit
from copy import deepcopy

from rcommander_interface.behaviors import Behaviors

polygon_viz = None


tfl = None
poly_proxy = None
hilight_proxy = None
clear_hilight_proxy = None
inited = False

SWITCH_WIDTH  = 0.08
SWITCH_HEIGHT = 0.125
BUTTON_WIDTH  = SWITCH_HEIGHT*2
BUTTON_HEIGHT = SWITCH_HEIGHT*1.25

WHITE  = ColorRGBA(255,255,255,0)
PURPLE = ColorRGBA(255,0  ,255,0)
HILIGHT_COLOR = PURPLE
# HILIGHT_COLOR = WHITE

poly_switch = None
poly_on = None
poly_off = None

behaviors = None

click_sub = None

polygons = dict()

def rectangle(header, width, height, center):
        poly = PolygonStamped()
        poly.header = deepcopy(header)
        x,y,z = center.x,center.y,center.z

        # this is for doing it in image coordinates
        # poly.polygon.points.append(Point(x,y-width/2,z-height/2))
        # poly.polygon.points.append(Point(x,y+width/2,z-height/2))
        # poly.polygon.points.append(Point(x,y+width/2,z+height/2))
        # poly.polygon.points.append(Point(x,y-width/2,z+height/2))

        # normal frame alignment
        poly.polygon.points.append(Point(x-width/2,y-height/2,z))
        poly.polygon.points.append(Point(x+width/2,y-height/2,z))
        poly.polygon.points.append(Point(x+width/2,y+height/2,z))
        poly.polygon.points.append(Point(x-width/2,y+height/2,z))


        return poly

def click_cb(msg):
        global poly_switch, poly_on, poly_off
        poly_switch = rectangle(msg.ray.header, SWITCH_WIDTH, SWITCH_HEIGHT, msg.ray.direction)
        on_center   = deepcopy(msg.ray.direction)
        on_center.x -= (SWITCH_WIDTH/2 + BUTTON_WIDTH/2)
        on_center.y += BUTTON_HEIGHT/2
        poly_on     = rectangle(msg.ray.header, BUTTON_WIDTH, BUTTON_HEIGHT, on_center)
        off_center = deepcopy(on_center)
        # off_center.y += SWITCH_WIDTH/2 + BUTTON_WIDTH/2
        off_center.y -= BUTTON_HEIGHT
        poly_off    = rectangle(msg.ray.header, BUTTON_WIDTH, BUTTON_HEIGHT, off_center)

        global polygons
        polygons["Light\nSwitch"] = poly_switch
        polygons["Turn On"]       = poly_on
        polygons["Turn Off"]      = poly_off

        # rospy.loginfo("Sent polygon %s" % str(poly))
        polygon_proxy("Light\nSwitch", True, poly_switch, WHITE)
        print 'published viz'
        # import pdb; pdb.set_trace()
        polygon_viz.publish(poly_switch)

        print poly_switch
        if inited:
                polygon_proxy("Turn On", True, poly_on, WHITE)
                polygon_proxy("Turn Off", True, poly_off, WHITE)
        rospy.loginfo("Sent polygons")

def if_click_cb(msg):
        global click_sub
        rospy.loginfo('if click cb')
        if click_sub: click_sub.unregister() # this is a terrible hack to ignore multiple click messages
        global inited
        
        polygon_proxy("Turn On", True, poly_on, WHITE)
        polygon_proxy("Turn Off", True, poly_off, WHITE)

        print msg.data
        if msg.data == "Light\nSwitch":
                inited = True
                polygon_proxy("Turn On", True, poly_on, HILIGHT_COLOR)
                polygon_proxy("Turn Off", True, poly_off, WHITE)
        if msg.data == "Turn On":
                behaviors.rocker_on()
                polygon_proxy(msg.data, True, polygons[msg.data], HILIGHT_COLOR)
        if msg.data == "Turn Off":
                behaviors.rocker_off()
                polygon_proxy(msg.data, True, polygons[msg.data], HILIGHT_COLOR)

        click_sub = rospy.Subscriber('/clicked_object', String, if_click_cb, queue_size=1)

def outline_switch():
        fake_click = ImageClick()
        # fake_click.ray.header.frame_id = '/4x4_0'
        fake_click.ray.header.frame_id = 'freeze_frame0'
        # fake_click.ray.header.frame_id = 'head_origin'
        fake_click.ray.header.stamp = rospy.Time.now()
        fake_click.ray.direction.x = -0.11
        fake_click.ray.direction.y =  0.0
        fake_click.ray.direction.z =  0.0
        click_cb(fake_click)

if __name__ == '__main__':
        rospy.init_node('click_to_if')
        tfl = tf.TransformListener()
        polygon_proxy = rospy.ServiceProxy('/draw_polygon', DrawPolygon)
        rospy.loginfo("Waiting for polygon service")
        polygon_proxy.wait_for_service()
        rospy.loginfo("polygon service ready")

        behaviors = Behaviors()

        polygon_viz = rospy.Publisher('/polygon_viz', PolygonStamped)
        
        # rospy.loginfo("Waiting for hilight service")
        # hilight_proxy = rospy.ServiceProxy("hilight_object")
        # rospy.loginfo("hilight service ready")
        # rospy.loginfo("waiting for clear hilight service")
        # clearhilight_proxy = rospy.ServiceProxy("clear_hilights")
        # rospy.loginfo("clear hilight service ready")
        outline_switch()
        # rospy.Subscriber('/interactive_manipulation_image_click', ImageClick, click_cb)
        click_sub = rospy.Subscriber('/clicked_object', String, if_click_cb, queue_size=1)
        rospy.spin()