Fix rviz marker issue

src/active_grasp/policy.py

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

src/active_grasp/rviz.py

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