tower_detect_viewer_server.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8; -*-
3 """
4 tower_detect_viewer_server.py
5 
6 communicating with the browser and controlling the visualization
7 
8 """
9 
10 import sys
11 import rospy
12 
13 PKG='jsk_pcl_ros'
14 
15 import imp
16 try:
17  imp.find_module(PKG)
18 except:
19  import roslib;roslib.load_manifest(PKG)
20 
21 from image_view2.msg import ImageMarker2, PointArrayStamped
22 from geometry_msgs.msg import Point
23 from std_msgs.msg import Int16
24 from std_msgs.msg import String
25 from std_msgs.msg import Header
26 from jsk_pcl_ros.msg import Int32Stamped
27 from jsk_recognition_msgs.srv import *
29 import tf
30 from draw_3d_circle import Drawer3DCircle
31 
32 from sensor_msgs.msg import Image
33 from cv_bridge import CvBridge, CvBridgeError
34 import cv
35 
36 class State:
37  INITIAL = 1
38  SELECT_TOWER = 2
39  CONFIRM = 3
40  START_TASK = 4
41  INITIALIZE_PROBLEM = 5
42  MOVE_LARGE_S_G = 6
43  MOVE_MIDDLE_S_I = 7
44  MOVE_LARGE_G_I = 8
45  MOVE_SMALL_S_G = 9
46  MOVE_LARGE_I_S = 10
47  MOVE_MIDDLE_I_G = 11
48  MOVE_LARGE_S_G = 12
49  def __init__(self, topic):
50  self.pub = rospy.Publisher(topic, Int16)
51  self.state_val = -1
52  def publish(self):
53  self.pub.publish(Int16(self.state_val))
54  def updateState(self, next_state):
55  self.state_val = next_state
56  self.publish()
57 
58 
60  # name of tower
61  TOWER_LOWEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_LOWEST
62  TOWER_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_MIDDLE
63  TOWER_HIGHEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_HIGHEST
64  PLATE_SMALL = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_SMALL
65  PLATE_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_MIDDLE
66  PLATE_LARGE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_LARGE
67  PLATE_HEIGHT_LOWEST = 0
68  PLATE_HEIGHT_MIDDLE = 1
69  PLATE_HEIGHT_HIGHEST = 2
70  ROBOT0_BASE_FRAME_ID = "/R1/L0"
71  ROBOT1_BASE_FRAME_ID = "/R2/L0"
72  def __init__(self):
73  # initialize the position of the tower
74  self.tower_position = {
75  self.TOWER_LOWEST: Point(),
76  self.TOWER_MIDDLE: Point(),
77  self.TOWER_HIGHEST: Point()
78  }
79  self.radius = rospy.get_param("radius", 0.075)
80  self.circle0 = Drawer3DCircle("/image_marker", 1, "/cluster00",
81  self.radius, [1, 0, 0])
82  self.circle1 = Drawer3DCircle("/image_marker", 2, "/cluster01",
83  self.radius, [0, 1, 0])
84  self.circle2 = Drawer3DCircle("/image_marker", 3, "/cluster02",
85  self.radius, [0, 0, 1])
86  self.circles = [self.circle0, self.circle1, self.circle2]
87  # bgr
88  self.color_indices = [[0, 0, 255], [0, 255, 0], [255, 0, 0]]
89  self.cluster_num = -1
90  self.circle0.advertise()
91  self.circle1.advertise()
92  self.circle2.advertise()
93  self.bridge = CvBridge()
94  self.state = State("/browser/state")
96  self.browser_click_sub = rospy.Subscriber("/browser/click",
97  Point,
98  self.clickCB)
99  self.browser_message_pub = rospy.Publisher("/browser/message",
100  String)
101  self.image_sub = rospy.Subscriber("/image_marked",
102  Image,
103  self.imageCB)
104  self.cluster_num_sub = rospy.Subscriber("/pcl_nodelet/clustering/cluster_num",
105  Int32Stamped,
106  self.clusterNumCB)
107  self.check_circle_srv = rospy.Service("/browser/check_circle",
108  CheckCircle,
109  self.checkCircleCB)
110  self.pickup_srv = rospy.Service("/browser/pickup",
111  TowerPickUp,
112  self.pickupCB)
113  self.state.updateState(State.INITIAL)
114 
115  # waiting for ik server
116  if rospy.get_param("~wait_robot_move_command", False):
117  rospy.loginfo("waiting for robot server")
118  rospy.wait_for_service("/browser/tower_robot_move_command")
119  self.robot_command = rospy.ServiceProxy("/browser/tower_robot_move_command", TowerRobotMoveCommand)
120  rospy.loginfo("connected to robot_move server")
121 
122  # initialize the position of the towers from TL
126  self.S_TOWER = self.TOWER_MIDDLE
127  self.G_TOWER = None
128  self.I_TOWER = None
129  def towerNameToFrameId(self, tower_name):
130  if tower_name == self.TOWER_LOWEST:
131  return "/cluster02"
132  elif tower_name == self.TOWER_MIDDLE:
133  return "/cluster01"
134  elif tower_name == self.TOWER_HIGHEST:
135  return "/cluster00"
136  else:
137  raise Exception("unknown tower: %d" % (tower_name))
138  def resolveTowerName(self, tower_id):
139  if tower_id == self.TOWER_LOWEST:
140  return "TOWER_LOWEST"
141  elif tower_id == self.TOWER_MIDDLE:
142  return "TOWER_MIDDLE"
143  elif tower_id == self.TOWER_HIGHEST:
144  return "TOWER_HIGHEST"
145  else:
146  raise Exception("unknown tower: %d" % (tower_id))
147  def resolvePlateName(self, plate_id):
148  if plate_id == self.PLATE_SMALL:
149  return "PLATE_SMALL"
150  elif plate_id == self.PLATE_MIDDLE:
151  return "PLATE_MIDDLE"
152  elif plate_id == self.PLATE_LARGE:
153  return "PLATE_LARGE"
154  else:
155  raise Exception("unknown plate id: %d" % (plate_id))
156  def resolvePlateHeightOffset(self, height_id):
157  """
158  return the offset of z-axis of `height_id'
159  """
160  return 0.0
161  def resolvePlateHeight(self, height_id):
162  if height_id == self.PLATE_HEIGHT_LOWEST:
163  return "lowest"
164  elif height_id == self.PLATE_HEIGHT_MIDDLE:
165  return "middle"
166  elif height_id == self.PLATE_HEIGHT_HIGHEST:
167  return "highest"
168  else:
169  raise Exception("unknown plate height: %d" % (height_id))
170  def robotBaseFrameId(self, index): #index is 0 or 1
171  if index == 0:
172  return self.ROBOT0_BASE_FRAME_ID
173  elif index == 1:
174  return self.ROBOT1_BASE_FRAME_ID
175  else:
176  raise Exception("unknown index: %d" % (index))
177  def updateTowerPosition(self, tower_name):
178  frame_id = self.towerNameToFrameId(tower_name)
179  rospy.loginfo("resolving %s" % (frame_id))
180  try:
181  (trans, rot) = self.tf_listener.lookupTransform("/origin", frame_id, rospy.Time(0))
182  rospy.loginfo("%s => %s: (%f, %f, %f)" % ("/origin", frame_id, trans[0], trans[1], trans[2]))
183  self.tower_position[tower_name].x = trans[0]
184  self.tower_position[tower_name].y = trans[1]
185  self.tower_position[tower_name].z = trans[2]
187  rospy.logerr("failed to lookup transform: %s => %s" % ("/origin", frame_id))
188  def clusterNumCB(self, msg):
189  self.cluster_num = msg.data
190  def moveRobot(self, plate, from_tower, to_tower, from_height, to_height):
191  robot_index = 0 #use the 1st robot first
192  robot_frame_id = self.robotBaseFrameId(robot_index)
193  rospy.loginfo("moving: %s from %s(%s) to %s(%s)" % (self.resolvePlateName(plate),
194  self.resolveTowerName(from_tower), self.resolvePlateHeight(from_height),
195  self.resolveTowerName(to_tower), self.resolvePlateHeight(to_height)))
196  from_target_position = self.tower_position[from_tower]
197  to_target_position = self.tower_position[to_tower]
198  self.robot_command(jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.ROBOT1,
199  plate,
200  from_tower, to_tower,
201  jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.OPTION_NONE)
202  # self.robot_server1(Header(), from_target_position, 0)
203  # self.robot_server1(Header(), to_target_position, 1)
204  def runMain(self):
205  # first of all, resolve tf and store the position of the tower
206  # but, we don't need to update `S' tower's position.
207  # update the tf value
208  self.updateTowerPosition(self.I_TOWER)
209  self.updateTowerPosition(self.G_TOWER)
210  self.state.updateState(State.MOVE_LARGE_S_G)
211  self.moveRobot(self.PLATE_LARGE,
212  self.S_TOWER, self.G_TOWER,
214 
215  self.state.updateState(State.MOVE_MIDDLE_S_I)
216  self.moveRobot(self.PLATE_MIDDLE,
217  self.S_TOWER, self.I_TOWER,
219 
220  self.state.updateState(State.MOVE_LARGE_G_I)
221  self.moveRobot(self.PLATE_LARGE,
222  self.G_TOWER, self.I_TOWER,
224 
225  self.state.updateState(State.MOVE_SMALL_S_G)
226  self.moveRobot(self.PLATE_SMALL,
227  self.S_TOWER, self.G_TOWER,
229 
230  self.state.updateState(State.MOVE_LARGE_I_S)
231  self.moveRobot(self.PLATE_LARGE,
232  self.I_TOWER, self.S_TOWER,
234 
235  self.state.updateState(State.MOVE_MIDDLE_I_G)
236  self.moveRobot(self.PLATE_MIDDLE,
237  self.I_TOWER, self.G_TOWER,
239 
240  self.state.updateState(State.MOVE_LARGE_S_G)
241  self.moveRobot(self.PLATE_LARGE,
242  self.S_TOWER, self.G_TOWER,
244  def pickupCB(self, req):
245  target_index = req.tower_index
246  # first of all, resolveing S, I and G name binding
247  # S is the START tower
248  # I is the INTERMEDIATE tower
249  # G is the GOAL tower
250  self.G_TOWER = req.tower_index
251  # lookup I
252  self.I_TOWER = (set([self.TOWER_LOWEST, self.TOWER_MIDDLE, self.TOWER_HIGHEST])
253  - set([self.G_TOWER, self.S_TOWER])).pop()
254 
255  # update the position of the tower
256  self.state.updateState(State.MOVE_LARGE_S_G)
257  self.state.publish()
258  self.runMain()
259  self.state.updateState(State.INITIAL)
260  # update S
261  self.S_TOWER = self.G_TOWER
262  return TowerPickUpResponse()
263  def checkCircleCB(self, req):
264  (width, height) = cv.GetSize(self.cv_image)
265  x = int(width * req.point.x)
266  y = int(height * req.point.y)
267  click_index = -1
268  if self.checkColor(self.cv_image[y, x], self.color_indices[0]):
269  click_index = self.TOWER_HIGHEST
270  elif self.checkColor(self.cv_image[y, x], self.color_indices[1]):
271  click_index = self.TOWER_MIDDLE
272  elif self.checkColor(self.cv_image[y, x], self.color_indices[2]):
273  click_index = self.TOWER_LOWEST
274  if click_index == self.S_TOWER:
275  msg = "the tower the user clicked equals to the start tower"
276  rospy.logerr(msg)
277  return CheckCircleResponse(False, click_index, msg)
278  else:
279  return CheckCircleResponse(click_index != -1, click_index, "")
280  def checkColor(self, image_color, array_color):
281  return (image_color[0] == array_color[0] and
282  image_color[1] == array_color[1] and
283  image_color[2] == array_color[2])
284  def clickCB(self, msg):
285  (width, height) = cv.GetSize(self.cv_image)
286  # msg.x and msg.y is on a relative coordinate (u, v)
287  x = int(width * msg.x)
288  y = int(height * msg.y)
289  output_str = str([x, y]) + " - " + str(self.cv_image[y, x])
290  click_index = -1
291  if self.checkColor(self.cv_image[y, x], self.color_indices[0]):
292  output_str = output_str + " cluster00 clicked"
293  click_index = self.TOWER_HIGHEST
294  elif self.checkColor(self.cv_image[y, x], self.color_indices[1]):
295  output_str = output_str + " cluster01 clicked"
296  click_index = self.TOWER_MIDDLE
297  elif self.checkColor(self.cv_image[y, x], self.color_indices[2]):
298  output_str = output_str + " cluster02 clicked"
299  click_index = self.TOWER_LOWEST
300  self.browser_message_pub.publish(String(output_str))
301  def imageCB(self, data):
302  try:
303  self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
304  except CvBridgeError as e:
305  print(e)
306  def publishState(self):
307  self.state.publish()
308  def spin(self):
309  while not rospy.is_shutdown():
310  for c in self.circles:
311  c.publish()
312  self.publishState()
313  rospy.sleep(1.0)
314 
315 def main():
316  rospy.init_node("tower_detect_viewer_server")
317  server = TowerDetectViewerServer()
318  server.spin()
319 
320 
321 if __name__ == "__main__":
322  main()
def moveRobot(self, plate, from_tower, to_tower, from_height, to_height)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47