tower_detect_viewer_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8; -*-
00003 """
00004 tower_detect_viewer_server.py
00005 
00006 communicating with the browser and controlling the visualization
00007 
00008 """
00009 
00010 import sys
00011 import rospy
00012 
00013 PKG='jsk_pcl_ros'
00014 
00015 import imp
00016 try:
00017     imp.find_module(PKG)
00018 except:
00019     import roslib;roslib.load_manifest(PKG)
00020 
00021 from image_view2.msg import ImageMarker2, PointArrayStamped
00022 from geometry_msgs.msg import Point
00023 from std_msgs.msg import Int16
00024 from std_msgs.msg import String
00025 from std_msgs.msg import Header
00026 from jsk_pcl_ros.msg import Int32Stamped
00027 from jsk_recognition_msgs.srv import *
00028 import jsk_recognition_msgs.srv
00029 import tf
00030 from draw_3d_circle import Drawer3DCircle
00031 
00032 from sensor_msgs.msg import Image
00033 from cv_bridge import CvBridge, CvBridgeError
00034 import cv
00035 
00036 class State:
00037     INITIAL = 1
00038     SELECT_TOWER = 2
00039     CONFIRM = 3
00040     START_TASK = 4
00041     INITIALIZE_PROBLEM = 5
00042     MOVE_LARGE_S_G = 6
00043     MOVE_MIDDLE_S_I = 7
00044     MOVE_LARGE_G_I = 8
00045     MOVE_SMALL_S_G = 9
00046     MOVE_LARGE_I_S = 10
00047     MOVE_MIDDLE_I_G = 11
00048     MOVE_LARGE_S_G = 12
00049     def __init__(self, topic):
00050         self.pub = rospy.Publisher(topic, Int16)
00051         self.state_val = -1
00052     def publish(self):
00053         self.pub.publish(Int16(self.state_val))
00054     def updateState(self, next_state):
00055         self.state_val = next_state
00056         self.publish()
00057 
00058         
00059 class TowerDetectViewerServer:
00060     # name of tower
00061     TOWER_LOWEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_LOWEST
00062     TOWER_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_MIDDLE
00063     TOWER_HIGHEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_HIGHEST
00064     PLATE_SMALL = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_SMALL
00065     PLATE_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_MIDDLE
00066     PLATE_LARGE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_LARGE
00067     PLATE_HEIGHT_LOWEST = 0
00068     PLATE_HEIGHT_MIDDLE = 1
00069     PLATE_HEIGHT_HIGHEST = 2
00070     ROBOT0_BASE_FRAME_ID = "/R1/L0"
00071     ROBOT1_BASE_FRAME_ID = "/R2/L0"
00072     def __init__(self):
00073         # initialize the position of the tower
00074         self.tower_position = {
00075             self.TOWER_LOWEST: Point(),
00076             self.TOWER_MIDDLE: Point(),
00077             self.TOWER_HIGHEST: Point()
00078         }
00079         self.radius = rospy.get_param("radius", 0.075)
00080         self.circle0 = Drawer3DCircle("/image_marker", 1, "/cluster00",
00081                                       self.radius, [1, 0, 0])
00082         self.circle1 = Drawer3DCircle("/image_marker", 2, "/cluster01",
00083                                       self.radius, [0, 1, 0])
00084         self.circle2 = Drawer3DCircle("/image_marker", 3, "/cluster02",
00085                                       self.radius, [0, 0, 1])
00086         self.circles = [self.circle0, self.circle1, self.circle2]
00087         # bgr
00088         self.color_indices = [[0, 0, 255], [0, 255, 0], [255, 0, 0]]
00089         self.cluster_num = -1
00090         self.circle0.advertise()
00091         self.circle1.advertise()
00092         self.circle2.advertise()
00093         self.bridge = CvBridge()
00094         self.state = State("/browser/state")
00095         self.tf_listener = tf.TransformListener()
00096         self.browser_click_sub = rospy.Subscriber("/browser/click", 
00097                                                   Point, 
00098                                                   self.clickCB)
00099         self.browser_message_pub = rospy.Publisher("/browser/message",
00100                                                   String)
00101         self.image_sub = rospy.Subscriber("/image_marked",
00102                                           Image,
00103                                           self.imageCB)
00104         self.cluster_num_sub = rospy.Subscriber("/pcl_nodelet/clustering/cluster_num",
00105                                                 Int32Stamped,
00106                                                 self.clusterNumCB)
00107         self.check_circle_srv = rospy.Service("/browser/check_circle",
00108                                               CheckCircle,
00109                                               self.checkCircleCB)
00110         self.pickup_srv = rospy.Service("/browser/pickup",
00111                                         TowerPickUp,
00112                                         self.pickupCB)
00113         self.state.updateState(State.INITIAL)
00114 
00115         # waiting for ik server
00116         if rospy.get_param("~wait_robot_move_command", False):
00117             rospy.loginfo("waiting for robot server")
00118             rospy.wait_for_service("/browser/tower_robot_move_command")
00119         self.robot_command = rospy.ServiceProxy("/browser/tower_robot_move_command", TowerRobotMoveCommand)
00120         rospy.loginfo("connected to robot_move server")
00121 
00122         # initialize the position of the towers from TL
00123         self.updateTowerPosition(self.TOWER_LOWEST)
00124         self.updateTowerPosition(self.TOWER_MIDDLE)
00125         self.updateTowerPosition(self.TOWER_HIGHEST)
00126         self.S_TOWER = self.TOWER_MIDDLE
00127         self.G_TOWER = None
00128         self.I_TOWER = None
00129     def towerNameToFrameId(self, tower_name):
00130         if tower_name == self.TOWER_LOWEST:
00131             return "/cluster02"
00132         elif tower_name == self.TOWER_MIDDLE:
00133             return "/cluster01"
00134         elif tower_name == self.TOWER_HIGHEST:
00135             return "/cluster00"
00136         else:
00137             raise Exception("unknown tower: %d" % (tower_name))
00138     def resolveTowerName(self, tower_id):
00139         if tower_id == self.TOWER_LOWEST:
00140             return "TOWER_LOWEST"
00141         elif tower_id == self.TOWER_MIDDLE:
00142             return "TOWER_MIDDLE"
00143         elif tower_id == self.TOWER_HIGHEST:
00144             return "TOWER_HIGHEST"
00145         else:
00146             raise Exception("unknown tower: %d" % (tower_id))
00147     def resolvePlateName(self, plate_id):
00148         if plate_id == self.PLATE_SMALL:
00149             return "PLATE_SMALL"
00150         elif plate_id == self.PLATE_MIDDLE:
00151             return "PLATE_MIDDLE"
00152         elif plate_id == self.PLATE_LARGE:
00153             return "PLATE_LARGE"
00154         else:
00155             raise Exception("unknown plate id: %d" % (plate_id))
00156     def resolvePlateHeightOffset(self, height_id):
00157         """
00158         return the offset of z-axis of `height_id'
00159         """
00160         return 0.0
00161     def resolvePlateHeight(self, height_id):
00162         if height_id == self.PLATE_HEIGHT_LOWEST:
00163             return "lowest"
00164         elif height_id == self.PLATE_HEIGHT_MIDDLE:
00165             return "middle"
00166         elif height_id == self.PLATE_HEIGHT_HIGHEST:
00167             return "highest"
00168         else:
00169             raise Exception("unknown plate height: %d" % (height_id))
00170     def robotBaseFrameId(self, index):    #index is 0 or 1
00171         if index == 0:
00172             return self.ROBOT0_BASE_FRAME_ID
00173         elif index == 1:
00174             return self.ROBOT1_BASE_FRAME_ID
00175         else:
00176             raise Exception("unknown index: %d" % (index))
00177     def updateTowerPosition(self, tower_name):
00178         frame_id = self.towerNameToFrameId(tower_name)
00179         rospy.loginfo("resolving %s" % (frame_id))
00180         try:
00181             (trans, rot) = self.tf_listener.lookupTransform("/origin", frame_id, rospy.Time(0))
00182             rospy.loginfo("%s => %s: (%f, %f, %f)" % ("/origin", frame_id, trans[0], trans[1], trans[2]))
00183             self.tower_position[tower_name].x = trans[0]
00184             self.tower_position[tower_name].y = trans[1]
00185             self.tower_position[tower_name].z = trans[2]
00186         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00187             rospy.logerr("failed to lookup transform: %s => %s" % ("/origin", frame_id))
00188     def clusterNumCB(self, msg):
00189         self.cluster_num = msg.data
00190     def moveRobot(self, plate, from_tower, to_tower, from_height, to_height):
00191         robot_index = 0                   #use the 1st robot first
00192         robot_frame_id = self.robotBaseFrameId(robot_index)
00193         rospy.loginfo("moving: %s from %s(%s) to %s(%s)" % (self.resolvePlateName(plate), 
00194                                                             self.resolveTowerName(from_tower), self.resolvePlateHeight(from_height),
00195                                                             self.resolveTowerName(to_tower), self.resolvePlateHeight(to_height)))
00196         from_target_position = self.tower_position[from_tower]
00197         to_target_position = self.tower_position[to_tower]
00198         self.robot_command(jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.ROBOT1,
00199                            plate,
00200                            from_tower, to_tower,
00201                            jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.OPTION_NONE)
00202         # self.robot_server1(Header(), from_target_position, 0)
00203         # self.robot_server1(Header(), to_target_position, 1)
00204     def runMain(self):
00205         # first of all, resolve tf and store the position of the tower
00206         # but, we don't need to update `S' tower's position.
00207         # update the tf value
00208         self.updateTowerPosition(self.I_TOWER)
00209         self.updateTowerPosition(self.G_TOWER)
00210         self.state.updateState(State.MOVE_LARGE_S_G)
00211         self.moveRobot(self.PLATE_LARGE, 
00212                        self.S_TOWER, self.G_TOWER, 
00213                        self.PLATE_HEIGHT_HIGHEST, self.PLATE_HEIGHT_LOWEST)
00214         
00215         self.state.updateState(State.MOVE_MIDDLE_S_I)
00216         self.moveRobot(self.PLATE_MIDDLE,
00217                        self.S_TOWER, self.I_TOWER,
00218                        self.PLATE_HEIGHT_MIDDLE, self.PLATE_HEIGHT_LOWEST)
00219         
00220         self.state.updateState(State.MOVE_LARGE_G_I)
00221         self.moveRobot(self.PLATE_LARGE, 
00222                        self.G_TOWER, self.I_TOWER, 
00223                        self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_MIDDLE)
00224         
00225         self.state.updateState(State.MOVE_SMALL_S_G)
00226         self.moveRobot(self.PLATE_SMALL, 
00227                        self.S_TOWER, self.G_TOWER, 
00228                        self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_LOWEST)
00229         
00230         self.state.updateState(State.MOVE_LARGE_I_S)
00231         self.moveRobot(self.PLATE_LARGE, 
00232                        self.I_TOWER, self.S_TOWER, 
00233                        self.PLATE_HEIGHT_MIDDLE, self.PLATE_HEIGHT_LOWEST)
00234         
00235         self.state.updateState(State.MOVE_MIDDLE_I_G)
00236         self.moveRobot(self.PLATE_MIDDLE, 
00237                        self.I_TOWER, self.G_TOWER, 
00238                        self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_MIDDLE)
00239         
00240         self.state.updateState(State.MOVE_LARGE_S_G)
00241         self.moveRobot(self.PLATE_LARGE, 
00242                        self.S_TOWER, self.G_TOWER, 
00243                        self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_HIGHEST)
00244     def pickupCB(self, req):
00245         target_index = req.tower_index
00246         # first of all, resolveing S, I and G name binding
00247         # S is the START tower
00248         # I is the INTERMEDIATE tower
00249         # G is the GOAL tower
00250         self.G_TOWER = req.tower_index
00251         # lookup I
00252         self.I_TOWER = (set([self.TOWER_LOWEST, self.TOWER_MIDDLE, self.TOWER_HIGHEST]) 
00253                         - set([self.G_TOWER, self.S_TOWER])).pop()
00254 
00255         # update the position of the tower
00256         self.state.updateState(State.MOVE_LARGE_S_G)
00257         self.state.publish()
00258         self.runMain()
00259         self.state.updateState(State.INITIAL)
00260         # update S
00261         self.S_TOWER = self.G_TOWER
00262         return TowerPickUpResponse()
00263     def checkCircleCB(self, req):
00264         (width, height) = cv.GetSize(self.cv_image)
00265         x = int(width * req.point.x)
00266         y = int(height * req.point.y)
00267         click_index = -1
00268         if self.checkColor(self.cv_image[y, x], self.color_indices[0]):
00269             click_index = self.TOWER_HIGHEST
00270         elif self.checkColor(self.cv_image[y, x], self.color_indices[1]):
00271             click_index = self.TOWER_MIDDLE
00272         elif self.checkColor(self.cv_image[y, x], self.color_indices[2]):
00273             click_index = self.TOWER_LOWEST
00274         if click_index == self.S_TOWER:
00275             msg = "the tower the user clicked equals to the start tower"
00276             rospy.logerr(msg)
00277             return CheckCircleResponse(False, click_index, msg)
00278         else:
00279             return CheckCircleResponse(click_index != -1, click_index, "")
00280     def checkColor(self, image_color, array_color):
00281         return (image_color[0] == array_color[0] and 
00282                 image_color[1] == array_color[1] and 
00283                 image_color[2] == array_color[2])
00284     def clickCB(self, msg):
00285         (width, height) = cv.GetSize(self.cv_image)
00286         # msg.x and msg.y is on a relative coordinate (u, v)
00287         x = int(width * msg.x)
00288         y = int(height * msg.y)
00289         output_str = str([x, y]) + " - " + str(self.cv_image[y, x])
00290         click_index = -1
00291         if self.checkColor(self.cv_image[y, x], self.color_indices[0]):
00292             output_str = output_str + " cluster00 clicked"
00293             click_index = self.TOWER_HIGHEST
00294         elif self.checkColor(self.cv_image[y, x], self.color_indices[1]):
00295             output_str = output_str + " cluster01 clicked"
00296             click_index = self.TOWER_MIDDLE
00297         elif self.checkColor(self.cv_image[y, x], self.color_indices[2]):
00298             output_str = output_str + " cluster02 clicked"
00299             click_index = self.TOWER_LOWEST
00300         self.browser_message_pub.publish(String(output_str))
00301     def imageCB(self, data):
00302         try:
00303             self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
00304         except CvBridgeError, e:
00305             print e
00306     def publishState(self):
00307         self.state.publish()
00308     def spin(self):
00309         while not rospy.is_shutdown():
00310             for c in self.circles:
00311                 c.publish()
00312             self.publishState()
00313             rospy.sleep(1.0)
00314 
00315 def main():
00316     rospy.init_node("tower_detect_viewer_server")
00317     server = TowerDetectViewerServer()
00318     server.spin()
00319 
00320 
00321 if __name__ == "__main__":
00322     main()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:45