4 tower_detect_viewer_server.py 6 communicating with the browser and controlling the visualization 19 import roslib;roslib.load_manifest(PKG)
21 from image_view2.msg
import ImageMarker2, PointArrayStamped
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
30 from draw_3d_circle
import Drawer3DCircle
32 from sensor_msgs.msg
import Image
33 from cv_bridge
import CvBridge, CvBridgeError
41 INITIALIZE_PROBLEM = 5
50 self.
pub = rospy.Publisher(topic, Int16)
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" 79 self.
radius = rospy.get_param(
"radius", 0.075)
90 self.circle0.advertise()
91 self.circle1.advertise()
92 self.circle2.advertise()
113 self.state.updateState(State.INITIAL)
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")
137 raise Exception(
"unknown tower: %d" % (tower_name))
140 return "TOWER_LOWEST" 142 return "TOWER_MIDDLE" 144 return "TOWER_HIGHEST" 146 raise Exception(
"unknown tower: %d" % (tower_id))
151 return "PLATE_MIDDLE" 155 raise Exception(
"unknown plate id: %d" % (plate_id))
158 return the offset of z-axis of `height_id' 169 raise Exception(
"unknown plate height: %d" % (height_id))
176 raise Exception(
"unknown index: %d" % (index))
179 rospy.loginfo(
"resolving %s" % (frame_id))
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]))
187 rospy.logerr(
"failed to lookup transform: %s => %s" % (
"/origin", frame_id))
190 def moveRobot(self, plate, from_tower, to_tower, from_height, to_height):
193 rospy.loginfo(
"moving: %s from %s(%s) to %s(%s)" % (self.
resolvePlateName(plate),
198 self.
robot_command(jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.ROBOT1,
200 from_tower, to_tower,
201 jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.OPTION_NONE)
210 self.state.updateState(State.MOVE_LARGE_S_G)
215 self.state.updateState(State.MOVE_MIDDLE_S_I)
220 self.state.updateState(State.MOVE_LARGE_G_I)
225 self.state.updateState(State.MOVE_SMALL_S_G)
230 self.state.updateState(State.MOVE_LARGE_I_S)
235 self.state.updateState(State.MOVE_MIDDLE_I_G)
240 self.state.updateState(State.MOVE_LARGE_S_G)
245 target_index = req.tower_index
256 self.state.updateState(State.MOVE_LARGE_S_G)
259 self.state.updateState(State.INITIAL)
262 return TowerPickUpResponse()
264 (width, height) = cv.GetSize(self.
cv_image)
265 x = int(width * req.point.x)
266 y = int(height * req.point.y)
274 if click_index == self.
S_TOWER:
275 msg =
"the tower the user clicked equals to the start tower" 277 return CheckCircleResponse(
False, click_index, msg)
279 return CheckCircleResponse(click_index != -1, click_index,
"")
281 return (image_color[0] == array_color[0]
and 282 image_color[1] == array_color[1]
and 283 image_color[2] == array_color[2])
285 (width, height) = cv.GetSize(self.
cv_image)
287 x = int(width * msg.x)
288 y = int(height * msg.y)
289 output_str = str([x, y]) +
" - " + str(self.
cv_image[y, x])
292 output_str = output_str +
" cluster00 clicked" 295 output_str = output_str +
" cluster01 clicked" 298 output_str = output_str +
" cluster02 clicked" 300 self.browser_message_pub.publish(String(output_str))
303 self.
cv_image = self.bridge.imgmsg_to_cv(data,
"bgr8")
304 except CvBridgeError
as e:
309 while not rospy.is_shutdown():
316 rospy.init_node(
"tower_detect_viewer_server")
321 if __name__ ==
"__main__":
def robotBaseFrameId(self, index)
def checkColor(self, image_color, array_color)
string ROBOT1_BASE_FRAME_ID
def clusterNumCB(self, msg)
string ROBOT0_BASE_FRAME_ID
def resolveTowerName(self, tower_id)
def checkCircleCB(self, req)
def resolvePlateHeight(self, height_id)
def towerNameToFrameId(self, tower_name)
def resolvePlateHeightOffset(self, height_id)
def updateTowerPosition(self, tower_name)
def resolvePlateName(self, plate_id)
def moveRobot(self, plate, from_tower, to_tower, from_height, to_height)
def __init__(self, topic)
def updateState(self, next_state)