00001
00002
00003 import os.path as osp
00004 import sys
00005
00006 import yaml
00007
00008 from geometry_msgs.msg import Vector3
00009 from geometry_msgs.msg import Quaternion
00010 from geometry_msgs.msg import Pose
00011 from jsk_rviz_plugins.srv import RequestMarkerOperate
00012 from jsk_rviz_plugins.msg import TransformableMarkerOperate
00013 from jsk_interactive_marker.msg import MarkerDimensions
00014 from jsk_interactive_marker.msg import PoseStampedWithName
00015 from jsk_interactive_marker.srv import GetTransformableMarkerFocus
00016 from jsk_interactive_marker.srv import GetTransformableMarkerFocusRequest
00017 from jsk_interactive_marker.srv import SetTransformableMarkerPose
00018 from jsk_interactive_marker.srv import SetTransformableMarkerPoseRequest
00019 from jsk_interactive_marker.srv import SetTransformableMarkerColor
00020 from jsk_interactive_marker.srv import SetTransformableMarkerColorRequest
00021 from jsk_interactive_marker.srv import SetMarkerDimensions
00022 from jsk_interactive_marker.srv import SetMarkerDimensionsRequest
00023 from jsk_recognition_utils.color import labelcolormap
00024 from jsk_recognition_msgs.msg import BoundingBox
00025 from jsk_recognition_msgs.msg import BoundingBoxArray
00026 import rospy
00027
00028
00029 class TransformableMarkersClient(object):
00030
00031 def __init__(self):
00032 self.server = rospy.resolve_name('~server')
00033
00034 self.config_file = rospy.get_param('~config_file')
00035 if not osp.exists(self.config_file):
00036 rospy.logfatal("config_file '{}' does not exist"
00037 .format(self.config_file))
00038 sys.exit(1)
00039 self.config = yaml.load(open(self.config_file))
00040
00041 self.req_marker = rospy.ServiceProxy(
00042 osp.join(self.server, 'request_marker_operate'),
00043 RequestMarkerOperate)
00044 self.req_color = rospy.ServiceProxy(
00045 osp.join(self.server, 'set_color'), SetTransformableMarkerColor)
00046 self.req_pose = rospy.ServiceProxy(
00047 osp.join(self.server, 'set_pose'), SetTransformableMarkerPose)
00048 self.req_dim = rospy.ServiceProxy(
00049 osp.join(self.server, 'set_dimensions'), SetMarkerDimensions)
00050 self.req_focus = rospy.ServiceProxy(
00051 osp.join(self.server, 'get_focus'),
00052 GetTransformableMarkerFocus)
00053 rospy.wait_for_service(self.req_marker.resolved_name)
00054 rospy.wait_for_service(self.req_color.resolved_name)
00055 rospy.wait_for_service(self.req_pose.resolved_name)
00056 rospy.wait_for_service(self.req_dim.resolved_name)
00057 rospy.wait_for_service(self.req_focus.resolved_name)
00058
00059 self.object_poses = {}
00060 self.object_dimensions = {}
00061
00062
00063
00064
00065 boxes = self.config['boxes']
00066 n_boxes = len(boxes)
00067 cmap = labelcolormap(n_boxes)
00068 for i in xrange(n_boxes):
00069 box = boxes[i]
00070 name = box['name']
00071 frame_id = box.get('frame_id', '/map')
00072 self.insert_marker(name, frame_id,
00073 type=TransformableMarkerOperate.BOX)
00074 self.set_color(name, (cmap[i][0], cmap[i][1], cmap[i][2], 0.5))
00075 dim = box.get('dimensions', [1, 1, 1])
00076 self.set_dimensions(name, dim)
00077 pos = box.get('position', [0, 0, 0])
00078 ori = box.get('orientation', [0, 0, 0, 1])
00079 self.set_pose(box['name'], box['frame_id'], pos, ori)
00080 self.object_poses[box['name']] = Pose(
00081 position=Vector3(*pos),
00082 orientation=Quaternion(*ori),
00083 )
00084 self.object_dimensions[box['name']] = Vector3(*dim)
00085 rospy.loginfo("Inserted transformable box '{}'.".format(name))
00086
00087 self.sub_dimensions = rospy.Subscriber(
00088 osp.join(self.server, 'marker_dimensions'),
00089 MarkerDimensions,
00090 self._dimensions_change_callback)
00091 self.sub_pose = rospy.Subscriber(
00092 osp.join(self.server, 'pose_with_name'),
00093 PoseStampedWithName,
00094 self._pose_change_callback)
00095
00096 do_auto_save = rospy.get_param('~config_auto_save', True)
00097 if do_auto_save:
00098 self.timer_save = rospy.Timer(
00099 rospy.Duration(1), self._save_callback)
00100
00101 self.pub_bboxes = rospy.Publisher(
00102 '~output/boxes', BoundingBoxArray, queue_size=1)
00103 self.timer_pub_bboxes = rospy.Timer(
00104 rospy.Duration(0.1), self._pub_bboxes_callback)
00105
00106 def insert_marker(self, name, frame_id, type):
00107 msg = TransformableMarkerOperate(
00108 name=name,
00109 type=type,
00110 action=TransformableMarkerOperate.INSERT,
00111 frame_id=frame_id,
00112 description=name,
00113 )
00114 self.req_marker(msg)
00115
00116 def set_color(self, name, rgba):
00117 req = SetTransformableMarkerColorRequest()
00118 req.target_name = name
00119 req.color.r = rgba[0]
00120 req.color.g = rgba[1]
00121 req.color.b = rgba[2]
00122 req.color.a = rgba[3]
00123 self.req_color(req)
00124
00125 def set_dimensions(self, name, dimensions):
00126 req = SetMarkerDimensionsRequest()
00127 req.target_name = name
00128 req.dimensions.x = dimensions[0]
00129 req.dimensions.y = dimensions[1]
00130 req.dimensions.z = dimensions[2]
00131 self.req_dim(req)
00132
00133 def set_pose(self, name, frame_id, position, orientation):
00134 req = SetTransformableMarkerPoseRequest()
00135 req.target_name = name
00136 req.pose_stamped.header.frame_id = frame_id
00137 req.pose_stamped.pose.position.x = position[0]
00138 req.pose_stamped.pose.position.y = position[1]
00139 req.pose_stamped.pose.position.z = position[2]
00140 req.pose_stamped.pose.orientation.x = orientation[0]
00141 req.pose_stamped.pose.orientation.y = orientation[1]
00142 req.pose_stamped.pose.orientation.z = orientation[2]
00143 req.pose_stamped.pose.orientation.w = orientation[3]
00144 self.req_pose(req)
00145
00146 def get_focus_marker(self):
00147 req = GetTransformableMarkerFocusRequest()
00148 res = self.req_focus(req)
00149 return res.target_name
00150
00151 def _dimensions_change_callback(self, msg):
00152 name = self.get_focus_marker()
00153 self.object_dimensions[name] = msg
00154
00155 def _pose_change_callback(self, msg):
00156 self.object_poses[msg.name] = msg.pose.pose
00157
00158 def _pub_bboxes_callback(self, event):
00159 bbox_array_msg = BoundingBoxArray()
00160 bbox_array_msg.header.frame_id = self.config['boxes'][0]['frame_id']
00161 bbox_array_msg.header.stamp = event.current_real
00162 for box in self.config['boxes']:
00163 bbox_msg = BoundingBox()
00164 pose = self.object_poses[box['name']]
00165 bbox_msg.header.frame_id = bbox_array_msg.header.frame_id
00166 bbox_msg.header.stamp = bbox_array_msg.header.stamp
00167 bbox_msg.pose = pose
00168 dimensions = self.object_dimensions[box['name']]
00169 bbox_msg.dimensions.x = dimensions.x
00170 bbox_msg.dimensions.y = dimensions.y
00171 bbox_msg.dimensions.z = dimensions.z
00172 bbox_array_msg.boxes.append(bbox_msg)
00173 self.pub_bboxes.publish(bbox_array_msg)
00174
00175 def _save_callback(self, event):
00176 for i, box in enumerate(self.config['boxes']):
00177 box_cfg = self.config['boxes'][i]
00178 box_cfg['name'] = box['name']
00179 pose = self.object_poses[box['name']]
00180 dimensions = self.object_dimensions[box['name']]
00181
00182 box_cfg.update({
00183 'frame_id': box['frame_id'],
00184 'dimensions': [
00185 dimensions.x,
00186 dimensions.y,
00187 dimensions.z,
00188 ],
00189 'position': [
00190 pose.position.x,
00191 pose.position.y,
00192 pose.position.z,
00193 ],
00194 'orientation': [
00195 pose.orientation.x,
00196 pose.orientation.y,
00197 pose.orientation.z,
00198 pose.orientation.w,
00199 ],
00200 })
00201 yaml.dump(self.config, open(self.config_file, 'w'))
00202
00203
00204 if __name__ == '__main__':
00205 rospy.init_node('transformable_markers_client')
00206 TransformableMarkersClient()
00207 rospy.spin()