def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" res = [] # make line strip points = [] colors = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) points.append(Point(x=t.x, y=t.y, z=t.z)) colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)) prev_t = t h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id if discrete: m_type = Marker.POINTS else: m_type = Marker.LINE_STRIP m = Marker(type=m_type, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 m.scale.x = 0.1 m.scale.y = 0.1 m.points = points m.colors = colors m.ns = "labeled_line" m.lifetime = rospy.Time(3000) res.append(m) for t, t_meta in tsdata_lst[::label_downsample]: text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 text.scale.z = 0.1 text.pose = T.poseFromTransform(t.transform) text.pose.position.z += zoffset text.color = ColorRGBA(0.0,0.0,1.0,1.0) text.text = t_meta["inserted_at"].strftime(fmt) text.ns = "labeled_line_text" text.lifetime = rospy.Time(3000) res.append(text) return res
def __init__(self): self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog') self.col_name = rospy.get_param('~col_name', 'pr1012') self.duration = rospy.get_param('~duration', 30) self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) self.pub = rospy.Publisher('/object_detection_marker_array', MarkerArray) self.marker_count = 0 objs = self.msg_store.query(type=Object6DPose._type, meta_query={"inserted_at": { "$gt": datetime.now() - timedelta(days=self.duration) }}, sort_query=[("$natural", -1)]) first_obj_meta = objs[0][1] trans = [tuple(self.msg_store.query(type=TransformStamped._type, meta_query={"inserted_at": { "$lt": first_obj_meta["inserted_at"] }}, sort_query=[("$natural", -1)], single=True))] trans += self.msg_store.query(type=TransformStamped._type, meta_query={"inserted_at": { "$gt": first_obj_meta["inserted_at"] }}, sort_query=[("$natural", -1)]) j = 0 m_arr = MarkerArray() for i in range(len(objs)): o,o_meta = objs[i] t,t_meta = trans[j] if o_meta["inserted_at"] > t_meta["inserted_at"]: j += 1 t,t_meta = trans[j] ps = T.transformPoseWithTransformStamped(o.pose, t) m_arr.markers += V.poseStampedToLabeledSphereMarker([ps, o_meta], o.type) while not rospy.is_shutdown(): self.pub.publish(m_arr) rospy.sleep(1.0) rospy.logdebug("publishing objectdetection_marker_array")
def transformStampedArrayToLabeledArrayMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id res = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: m = Marker(type=Marker.ARROW, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) m.points = [Point(x=prev_t.x,y=prev_t.y,z=prev_t.z), Point(x=(prev_t.x + t.x) / 2.,y=(prev_t.y + t.y) /2.,z=(prev_t.z + t.z) /2.)] m.ns = "labeled_line" m.lifetime = rospy.Time(3000) m.scale.x, m.scale.y, m.scale.z = 0.02, 0.06, 0.1 m.color = ColorRGBA(rgb[0],rgb[1],rgb[2],1.0) if dist < 0.65: res.append(m) prev_t = t for t, t_meta in tsdata_lst[::label_downsample]: text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 text.scale.z = 0.1 text.pose = T.poseFromTransform(t.transform) text.pose.position.z += zoffset text.color = ColorRGBA(0.0,0.0,1.0,1.0) text.text = t_meta["inserted_at"].strftime(fmt) text.ns = "labeled_line_text" text.lifetime = rospy.Time(3000) res.append(text) return res