monoforce demo: publishing real path from dataset as well

Author: rlnav

config/rviz/monoforce2.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
       Splitter Ratio: 0.5
-    Tree Height: 555
+    Tree Height: 179
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -76,7 +76,7 @@ Visualization Manager:
       Head Length: 0.03999999910593033
       Length: 0.30000001192092896
       Line Style: Billboards
-      Line Width: 0.10000000149011612
+      Line Width: 0.029999999329447746
       Name: Path
       Offset:
         X: 0
@@ -91,6 +91,30 @@ Visualization Manager:
       Topic: /path
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 252; 233; 79
+      Enabled: true
+      Head Diameter: 0.07999999821186066
+      Head Length: 0.03999999910593033
+      Length: 0.30000001192092896
+      Line Style: Billboards
+      Line Width: 0.029999999329447746
+      Name: Path GT
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.019999999552965164
+      Shaft Length: 0.10000000149011612
+      Topic: /path_gt
+      Unreliable: false
+      Value: true
     - Alpha: 0.30000001192092896
       Autocompute Intensity Bounds: false
       Class: grid_map_rviz_plugin/GridMap
@@ -445,6 +469,7 @@ Visualization Manager:
         CameraInfo Rear: true
         CameraInfo Right: true
         Cloud: true
+        Forces: true
         Grid: true
         GridMap Lidar: true
         GridMap Pred Diff: true
@@ -456,6 +481,7 @@ Visualization Manager:
         Image Rear: true
         Image Right: true
         Path: true
+        Path GT: true
         RobotModel: true
         TF: true
         Value: true
@@ -471,6 +497,7 @@ Visualization Manager:
       Unreliable: false
       Value: false
       Visibility:
+        "": true
         Camera Front: true
         Camera Rear: true
         Camera Right: true
@@ -490,6 +517,7 @@ Visualization Manager:
         Image Rear: true
         Image Right: true
         Path: true
+        Path GT: true
         RobotModel: true
         TF: true
         Value: true
@@ -505,6 +533,7 @@ Visualization Manager:
       Unreliable: false
       Value: false
       Visibility:
+        "": true
         Camera Front: true
         Camera Left: true
         Camera Right: true
@@ -524,6 +553,7 @@ Visualization Manager:
         Image Rear: true
         Image Right: true
         Path: true
+        Path GT: true
         RobotModel: true
         TF: true
         Value: true
@@ -539,6 +569,7 @@ Visualization Manager:
       Unreliable: false
       Value: false
       Visibility:
+        "": true
         Camera Front: true
         Camera Left: true
         Camera Rear: true
@@ -558,10 +589,19 @@ Visualization Manager:
         Image Rear: true
         Image Right: true
         Path: true
+        Path GT: true
         RobotModel: true
         TF: true
         Value: true
       Zoom Factor: 1
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /forces
+      Name: Forces
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 238; 238; 236
@@ -652,7 +692,7 @@ Window Geometry:
     collapsed: false
   Image Right:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -663,4 +703,4 @@ Window Geometry:
     collapsed: true
   Width: 683
   X: 543
-  Y: 124
+  Y: 87

nodes/monoforce

@@ -58,6 +58,7 @@ class DataVis:
         self.gm_diff_pub = rospy.Publisher('grid_map_diff', GridMap, queue_size=1)
         self.cloud_pub = rospy.Publisher('cloud', PointCloud2, queue_size=1)
         self.path_pub = rospy.Publisher('path', Path, queue_size=1)
+        self.path_gt_pub = rospy.Publisher('path_gt', Path, queue_size=1)
         self.img_pubs = [rospy.Publisher('%s/image' % cam, Image, queue_size=1) for cam in self.camera_frames]
         self.caminfo_pubs = [rospy.Publisher('%s/camera_info' % cam, CameraInfo, queue_size=1) for cam in self.camera_frames]
         self.tf_broadcast = tf2_ros.TransformBroadcaster()
@@ -249,6 +250,9 @@ class DataVis:
         imgs, rots, trans, intrins, post_rots, post_trans, hm_lidar, hm_traj, map_pose, lidar_pts = ds[sample_i]
         points = lidar_pts.numpy().T
 
+        traj = ds.get_traj(sample_i)
+        poses_gt = traj['poses']
+
         imgs_raw = []
         Ks = []
         for cam in self.camera_frames:
@@ -305,9 +309,11 @@ class DataVis:
                 rospy.loginfo('Predicted forces shape: %s' % str(forces.shape))
                 robot_points = robot_points.cpu().numpy()
                 rospy.loginfo('Robot contact points shape: %s' % str(robot_points.shape))
-                # publish path
+                # publish paths
                 path_msg = to_path(poses, frame_id=self.robot_initial_frame, stamp=stamp)
+                path_gt_msg = to_path(poses_gt, frame_id=self.robot_initial_frame, stamp=stamp)
                 self.path_pub.publish(path_msg)
+                self.path_gt_pub.publish(path_gt_msg)
 
             # robot pose in map frame
             tf = to_tf(map_pose, self.map_frame, self.robot_initial_frame, stamp)