Clean whitespace in d435i_* nodes + fix frustrum marker msg warning

stretch_core/nodes/d435i_accel_correction

@@ -8,11 +8,11 @@ class D435iAccelCorrectionNode:
     def __init__(self):
         self.num_samples_to_skip = 4
         self.sample_count = 0
-        
+
     def accel_callback(self, accel):
         self.accel = accel
         self.sample_count += 1
-        if (self.sample_count % self.num_samples_to_skip) == 0: 
+        if (self.sample_count % self.num_samples_to_skip) == 0:
             # This can avoid issues with the D435i's time stamps being too
             # far ahead for TF.
             self.accel.header.stamp = rospy.Time.now()
@@ -26,7 +26,7 @@ class D435iAccelCorrectionNode:
 
             self.corrected_accel_pub.publish(self.accel)
 
-            
+
     def main(self):
         rospy.init_node('D435iAccelCorrectionNode')
         self.node_name = rospy.get_name()
@@ -34,7 +34,7 @@ class D435iAccelCorrectionNode:
 
         self.topic_name = '/camera/accel/sample'
         self.accel_subscriber = rospy.Subscriber(self.topic_name, Imu, self.accel_callback)
-        
+
         self.corrected_accel_pub = rospy.Publisher('/camera/accel/sample_corrected', Imu, queue_size=1)
 
 

stretch_core/nodes/d435i_configure

@@ -10,21 +10,21 @@ class D435iConfigureNode:
         self.rate = 1.0
         self.visual_preset = None
         self.mode_lock = threading.Lock()
-        
+
     def turn_on_default_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 1
             self.locked_mode_name = 'Default'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def turn_on_high_accuracy_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 3
             self.locked_mode_name = 'High Accuracy'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def default_mode_service_callback(self, request):
         self.turn_on_default_mode()
         return TriggerResponse(
@@ -58,9 +58,9 @@ class D435iConfigureNode:
         while not rospy.is_shutdown():
             rate.sleep()
 
-            
+
 if __name__ == '__main__':
-    try: 
+    try:
         node = D435iConfigureNode()
         node.main()
     except KeyboardInterrupt:

stretch_core/nodes/d435i_frustum_visualizer

@@ -10,7 +10,7 @@ from visualization_msgs.msg import MarkerArray
 from geometry_msgs.msg import Transform, Pose, Vector3, Quaternion, Point
 from sensor_msgs.msg import CameraInfo
 
-    
+
 class FrustumNode:
     def __init__(self):
         self.color_count = 0
@@ -18,8 +18,8 @@ class FrustumNode:
         self.camera_types = ['depth', 'color']
         # only publish a single frustum for every N camera info
         # messages received
-        self.skip_publishing = 5 
-        
+        self.skip_publishing = 5
+
         # minimum-Z depth for D435i
         # 1280x720 0.28  m
         # 848x480  0.195 m
@@ -43,12 +43,12 @@ class FrustumNode:
         if (self.depth_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='depth')
         self.depth_count = self.depth_count + 1
-        
+
     def color_camera_info_callback(self, camera_info):
         if (self.color_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='color')
         self.color_count = self.color_count + 1
-        
+
     def camera_info_callback(self, camera_info, camera_type=None):
         if camera_type not in self.camera_types:
             print('WARNING: FrustumNode camera_info_callback camera_type = {0} unrecognized. Valid types = {1}.'.format(camera_type, self.camera_types))
@@ -101,28 +101,28 @@ class FrustumNode:
         marker.type = marker.TRIANGLE_LIST
         marker.action = marker.ADD
         marker.lifetime = rospy.Duration(0.5) #0.2
-        marker.text = 'D435i_frustum'
         marker.header.frame_id = frame_id
         marker.header.stamp = timestamp
         marker.id = 0
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
-        if camera_type == 'depth': 
+        marker.pose.orientation.w = 1.0
+        if camera_type == 'depth':
             # frustum color and transparency
             # gray
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 1.0
             marker.color.a = 0.4
-        elif camera_type == 'color': 
+        elif camera_type == 'color':
             # frustum color and transparency
             # yellow
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             marker.color.a = 0.4
-            
+
         def adjacent_corners(c0, c1):
             # return True if the corners are adjacent to one another and False otherwise
             faces0 = c0[1]
@@ -170,23 +170,23 @@ class FrustumNode:
 
         points = []
         quad_ids = ['near', 'far', 'top', 'bottom', 'left', 'right']
-        for s in quad_ids: 
+        for s in quad_ids:
             quad_xyz = corners_to_quad(frustum_corners_xyz, s)
             triangles = quad_to_triangles(quad_xyz)
             points.extend(triangles)
         marker.points = points
-        
+
         if camera_type == 'depth':
             self.depth_frustum_marker_pub.publish(marker)
         elif camera_type == 'color':
             self.color_frustum_marker_pub.publish(marker)
 
-        
+
     def main(self):
         rospy.init_node('FrustumNode')
         self.node_name = rospy.get_name()
         rospy.loginfo("{0} started".format(self.node_name))
-        
+
         self.color_camera_topic = '/camera/color/camera_info'
         self.depth_camera_topic = '/camera/depth/camera_info'
         self.depth_camera_info_subscriber = rospy.Subscriber(self.depth_camera_topic, CameraInfo, self.depth_camera_info_callback)