diff --git a/stretch_core/nodes/d435i_accel_correction b/stretch_core/nodes/d435i_accel_correction index 618312d..2bb6d5c 100755 --- a/stretch_core/nodes/d435i_accel_correction +++ b/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) diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure index 9c478b4..63f4b8a 100755 --- a/stretch_core/nodes/d435i_configure +++ b/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: diff --git a/stretch_core/nodes/d435i_frustum_visualizer b/stretch_core/nodes/d435i_frustum_visualizer index 132226d..3b00a21 100755 --- a/stretch_core/nodes/d435i_frustum_visualizer +++ b/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)