hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix rviz marker issue

Browse Source
This commit is contained in:
Michel Breyer 2021-12-07 17:48:11 +01:00
parent 7b826434d2
commit 47d5840aa6
2 changed files with 28 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/active_grasp/policy.py
View File

@ -138,8 +138,7 @@ class MultiViewPolicy(Policy):
def integrate(self, img, x, q): def integrate(self, img, x, q):
self.views.append(x) self.views.append(x)
self.vis.path(self.base_frame, self.views) self.vis.path(self.base_frame, self.intrinsic, self.views)
self.vis.views(self.base_frame, self.intrinsic, self.views[::4])
with Timer("tsdf_integration"): with Timer("tsdf_integration"):
self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task) self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)

54
src/active_grasp/rviz.py
View File

@ -46,30 +46,48 @@ class Visualizer(vgn.rviz.Visualizer):
markers.append(marker) markers.append(marker)
self.draw(markers) self.draw(markers)
def path(self, frame, poses): def path(self, frame, intrinsic, views):
color = blue markers = []
points = [p.translation for p in poses] points = [p.translation for p in views]
spheres = create_sphere_list_marker( spheres = create_sphere_list_marker(
frame, frame,
Transform.identity(), Transform.identity(),
np.full(3, 0.01), np.full(3, 0.008),
color, blue,
points, points,
"path", "path",
0, 0,
) )
markers = [spheres] markers.append(spheres)
if len(poses) > 1:
if len(views) > 1:
lines = create_line_strip_marker( lines = create_line_strip_marker(
frame, frame,
Transform.identity(), Transform.identity(),
[0.005, 0.0, 0.0], [0.002, 0.0, 0.0],
color, blue,
points, points,
"path", "path",
1, 1,
) )
markers.append(lines) markers.append(lines)
for i, view in enumerate(views[::4]):
markers.append(
create_view_marker(
frame,
view,
[0.002, 0.0, 0.0],
blue,
intrinsic,
0.0,
0.02,
ns="views",
id=i,
)
)
self.draw(markers) self.draw(markers)
def point(self, frame, point): def point(self, frame, point):
@ -94,24 +112,6 @@ class Visualizer(vgn.rviz.Visualizer):
) )
self.draw([marker]) self.draw([marker])
def views(self, frame, intrinsic, views):
markers = []
for i, view in enumerate(views):
markers.append(
create_view_marker(
frame,
view,
[0.002, 0.0, 0.0],
blue,
intrinsic,
0.0,
0.02,
ns="views",
id=i,
)
)
self.draw(markers)
def create_view_marker(frame, pose, scale, color, intrinsic, near, far, ns="", id=0): def create_view_marker(frame, pose, scale, color, intrinsic, near, far, ns="", id=0):
marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id) marker = create_marker(Marker.LINE_LIST, frame, pose, scale, color, ns, id)