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
