#!/usr/bin/env python # # depthtolaserscan.py # # Convert an depth image into a laser scan. This assumes the camera # is mounted horizontally! # # Node: /depthtolaserscan # # Params: ~ground_frame_id Frame at ground level, with Z vertical up # ~laser_frame_id Frame for laser scan # (x forward, Z vertical up, Y left) # ~min_height Min height for object to "be detected" # ~max_height Max height for object to "be detected" # # Subscribe: /camera/depth/... # .../camera_info sensor_msgs/CameraInfo # .../image_rect_raw sensor_msgs/Image # # Publish: /scan sensor_msgs/LaserScan # # TF Required: ground_frame -> camera_frame # # TF Provided: camera_frame -> laser_frame (static) # import math import numpy as np import rospy import tf2_ros from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import CameraInfo from sensor_msgs.msg import Image from sensor_msgs.msg import LaserScan # # Constants # TIMEOUT = 5.0 # Timeout for the initialization INFO_TOPIC = '/camera/depth/camera_info' IMAGE_TOPIC = '/camera/depth/image_rect_raw' SCAN_TOPIC = '/scan' # Reporting a distance below the minimum implies the scanner was not # able to get any information, equivalent to an error. Reporting a # distance above the maximum implies there are no contacts (between # the minimum to maximum distance), everything is clear. RANGE_BAD = 0.000 # Range considered zero/unknown RANGE_MINIMUM = 0.050 # Minimum valid contact distance RANGE_MAXIMUM = 4.000 # Maximum valid contact distance RANGE_NOCONTACT = 5.000 # Valid range is clear RANGE_INFINITE = 6.000 # Never reported, used in sorting ###################################################################### # Mathematical/Algorithm Utilities ###################################################################### # # Before we derive the algorithm, we define two coordinate frames in # the same location, but with different axes. The Image frame is # horizontal, but pitched up. The Laser frame is aligned with the # ground (Z vertical). # # Image Frame: X image right horizontal, Y image down, Z into image # Laser Frame: X forward horizontal, Y left horizontal, Z vertical up # # A point in space can be decribed in image frame (xi, yi, zi) or # laser frame (xl, yl, zl) and converted with: # # xl = cos(pitch) zi + sin(pitch) yi = cp * zi + sp * yi # yl = - xi = -xi # zl = sin(pitch) zi - cos(pitch) yi = sp * zi - cp * yi # # xi = yl = yl # yi = sin(pitch) xl - cos(pitch) zl = sp * xl - cp * zl # zi = cos(pitch) xl + sin(pitch) zl = cp * xl + sp * zl # # The point (given in image frame) can also be mapped to the image # coordinates (u,v) and depth (d): # # u = cu + fu * (xi/zi) # v = cv + fv * (yi/zi) # d = zi # # where cu/cv are the center of the image, and fu/fv are the focal # lengths expressed in pixel size. This again can be inverted as: # # xi = d * (u-cu)/fu # yi = d * (v-cv)/fv # zi = d # # And then the image coordinates can be directly mapped to the # laser frame (vertical up): # # xl = d * (cp + sp * vbar) = r * cos(alpha) # yl = d * ( -ubar ) = r * sin(alpha) # zl = d * (sp - cp * vbar) = r * tan(gamma) # # where r = range in horz plane (for laser scan) # alpha = angle in horz plane (for laser scan) # gamma = angle above horz plane (to be projected down) # # r = sqrt(xl^2 + yl^2) = d * sqrt(ubar^2 + (cp + sp*vbar)^2) # tan(alpha) = yl/xl = -ubar/(cp+sp*vbar) # tan(gamma) = zl/r = cos(alpha) * (sp - cp*vbar) / (cp + sp*vbar) # # or in reverse direction # # vbar = (calpha * sp - tgamma * cp) / (calpha * cp + tgamma * sp) # vbar_horizon = sp/cp # vbar_vanish = -cp/sp # # ubar = - tan(alpha) * (cp + sp*vbar) # ubar_horizon = - tan(alpha) / cp # ###################################################################### # General Setup Utilities ###################################################################### # # Grab the Camera Info # def grabCameraInfo(): # Grab a single message of the camera_info topic. infomsg = rospy.wait_for_message(INFO_TOPIC, CameraInfo, TIMEOUT) # Note the image frame. imageframe = infomsg.header.frame_id # Note the width and height. Nu = infomsg.width Nv = infomsg.height # Pull the image resolution and center numbers from the projection # matrix. note this maps an (x,y,z) point to (u=column,v=row) as: # u = cu + fu * (x/z) # v = cv + fv * (y/z) fu = infomsg.P[0] cu = infomsg.P[2] fv = infomsg.P[5] cv = infomsg.P[6] # Report. rospy.loginfo("Projection: u = %7.3f + %7.3f * (x/z)" % (cu, fu)) rospy.loginfo(" v = %7.3f + %7.3f * (y/z)" % (cv, fv)) rospy.loginfo("Image %dx%d = %.0fx%.0f deg FOV" % (Nu, Nv, 2 * math.atan(Nu/2/fu) * 180/math.pi, 2 * math.atan(Nv/2/fv) * 180/math.pi)) # Return the data. return (imageframe, cu, fu, cv, fv, Nu, Nv) # # Grab the Camera Height (above ground) and Pitch angle (upward) # def grabCameraPose(groundframe, imageframe): # Create a TF buffer and listener. tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) # Try to grab the ground to image transform. Use rospy.Time(0) to # grab the latest transform, as opposed to the transform at a # particular time. Use a timeout, in case no transforms appear. try: # Grab the transform as a TransformStamped message. msg = tfBuffer.lookup_transform(groundframe, imageframe, rospy.Time(0), rospy.Duration(5.0)) except Exception as ex: rospy.logerr("Unable to get the '%s' to '%s' transform!" % (groundframe, imageframe)) rospy.logerr("Have you started the robot_state_publisher?") raise ex # Remove the listener (don't need any more information). listener.unregister() # Extract the rotation and the vertical components of the X/Y/Z # axes, knowing the ground frame's Z axis is vertical. r = msg.transform.rotation xvertical = 2*(r.x*r.z - r.w*r.y) # X = Image right yvertical = 2*(r.y*r.z + r.w*r.x) # Y = Image down zvertical = 1-2*(r.x**2 + r.y**2) # Z = Image forward # Check the camera (Image-right) being horizontal. if (abs(xvertical) > 1e-9): errstr = "The depth camera is not mounted horizontally!" rospy.logerr(errstr) raise rospy.ROSException(errstr) # Grab the pitch angle. spitch = zvertical # Vertical component of Z axis cpitch = -yvertical # Vertical component of -Y axis pitch = math.atan2(spitch, cpitch) # Also grab the height. hcam = msg.transform.translation.z # Report. rospy.loginfo("The depth camera is located %.2fmm above ground" % (hcam*1000)) rospy.loginfo("The depth camera is pitched up %6.3fdeg (%4.3frad)" % (pitch * 180/math.pi, pitch)) # Return the data. return (hcam, pitch, spitch, cpitch) # # Broadcast the Laser Frame, rotated down by Pitch angle # def broadcastLaserFrame(imageframe, laserframe, pitch): # Create the broadcaster (for a static transform). broadcaster = tf2_ros.StaticTransformBroadcaster() # Precompute the sin/cos s = math.sin(pitch/2) c = math.cos(pitch/2) # Prepare the transform message. Rotate such that the laser frame # is X horizontal along image axis, Y left, Z up (vertical). msg = TransformStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = imageframe msg.child_frame_id = laserframe msg.transform.translation.x = 0.0 msg.transform.translation.y = 0.0 msg.transform.translation.z = 0.0 msg.transform.rotation.x = 0.5 * (c - s) msg.transform.rotation.y = 0.5 * (s - c) msg.transform.rotation.z = 0.5 * (s + c) msg.transform.rotation.w = 0.5 * (s + c) # Broadcast. broadcaster.sendTransform(msg) ###################################################################### # Prepare the Sampling ###################################################################### # # Depth to Range Factor # # Multiply a depth (in image) to get matching range for laser scan. # def depthToRange(u,v): return math.sqrt((cpitch + spitch * (v-cv)/fv)**2 + ((u-cu)/fu)**2) # # Max Depth for Contact # # Consider obstacles between hmin and hmax (relative to hcam). So # once the depth exceeds the corresponding depth (in the given # direction), we know there is no contact. # def maxDepthForContact(u,v): # Depth corresponding to max contact range (in given direction). dmax = RANGE_MAXIMUM / depthToRange(u,v) # Also consider the depth limit due to height, looking up or down. a = spitch - cpitch * (v-cv)/fv if (a > 0): return min(dmax, (hmax-hcam)/a) elif (a < 0): return min(dmax, (hmin-hcam)/a) else: return dmax # # Define the Sample List (including pre-computed information) # def setSampleList(): # Start with an empty list, add each angle. samplelist = [] for i in range(angle_num): angle = angle_min + i * angle_inc ta = math.tan(angle) # For each angle, collect a list of sample points. samples = [] for v in range(0, Nv, dv): vbar = (float(v) - cv) / fv ubar = - ta * (cpitch + spitch * vbar) u = int(round(cu + fu * ubar)) if (u < 0) or (u >= Nu): break # Precompute/save the important information. samples.append((u, v, 1000*maxDepthForContact(u,v), depthToRange(u,v)/1000)) # Push the list of sample points for this angle. samplelist.append(samples) # Return the complete sample list return samplelist ###################################################################### # Callback - Sampling Algorithm ###################################################################### # # Depth Image Callback # # Convert the depth image into a laser scan # def callback_image(imagemsg): # Record the arrival time. The imagemsg timestamp *should* tell # us when the underlying data was sampled, but the RealSense # timestamps are incorrect. So this is our best guess. # Potentially we could shift a small delay back in time, as we # know it takes finite time to process the data and get it here. timestamp = rospy.Time.now() # Report (for debugging). rospy.loginfo("Image #%4d at %10d secs" % (imagemsg.header.seq, imagemsg.header.stamp.secs)) # Check the image width/height. if (imagemsg.width != Nu) or (imagemsg.height != Nv): rospy.logerr("Depth image has changed size (%dx%d) vs (%dx%d)!" % (imagemsg.width, imagemsg.height, Nu, Nv)) return # Check the data encoding. encoding = '16UC1' if (imagemsg.encoding != encoding): rospy.logerr("Depth image type encoding '%s' different from '%s'!" % (imagemsg.encoding, encoding)) return # Extract the depth image information (distance in mm as uint16). depth = np.frombuffer(imagemsg.data, np.uint16).reshape(Nv, Nu) # # Testing... # u = int(round(cu)) # v = int(round(cv)) # d = depth[v][u] # rospy.loginfo("Distance at (row %d, col %d) = %dmm" % (v, u, d)) # Create the range list. ranges = [RANGE_BAD] * angle_num # Compute range for each angle, as minimum over all sample points. for i in range(angle_num): minrange = RANGE_INFINITE for (u, v, maxdepth, depthtorange) in samplelist[i]: if (depth[v][u] > maxdepth): minrange = min(minrange, RANGE_NOCONTACT) elif (depth[v][u] > 0.0): minrange = min(minrange, depth[v][u] * depthtorange) if (minrange == RANGE_INFINITE): minrange = RANGE_BAD ranges[i] = minrange # Create scan msg. scanmsg = LaserScan() scanmsg.header.stamp = timestamp scanmsg.header.frame_id = laserframe scanmsg.angle_min = angle_min scanmsg.angle_max = angle_max scanmsg.angle_increment = angle_inc scanmsg.time_increment = 0.0 scanmsg.scan_time = 1/30.0 scanmsg.range_min = RANGE_MINIMUM # minimum range value [m] scanmsg.range_max = RANGE_MAXIMUM # maximum range value [m] scanmsg.ranges = ranges # Publish. pub.publish(scanmsg) ###################################################################### # Main Code ###################################################################### if __name__ == "__main__": # Initialize the ROS node. rospy.init_node('depthtolaserscan') # Grab the Ros parameters. groundframe = rospy.get_param('~ground_frame_id', 'base') laserframe = rospy.get_param('~laser_frame_id', 'laser') hmin = rospy.get_param('~min_height', 0.010) hmax = rospy.get_param('~max_height', 0.250) horzsamples = int(math.ceil(rospy.get_param('~horz_samples', -1))) vertsamples = int(math.ceil(rospy.get_param('~vert_samples', -1))) # Grab the camera image info. rospy.loginfo("Getting depth camera info...") (imageframe, cu, fu, cv, fv, Nu, Nv) = grabCameraInfo() rospy.loginfo("Ground frame '%s'" % groundframe) rospy.loginfo("Image frame '%s'" % imageframe) rospy.loginfo("Laser frame '%s'" % laserframe) # Grab the camera height and pitch angle. rospy.loginfo("Getting depth camera pose...") (hcam, pitch, spitch, cpitch) = grabCameraPose(groundframe, imageframe) # Generate the laser frame (in which to publish the scan data). broadcastLaserFrame(imageframe, laserframe, pitch) # Determine the angles to sample, and the number of lines/angle. angle_num = (1+Nu/2) if horzsamples <= 0 else horzsamples angle_min = math.atan(-cpitch * (Nu-1-cu)/fu) angle_max = math.atan(-cpitch * ( -cu)/fu) angle_inc = (angle_max - angle_min) / (angle_num - 1) dv = 2 if vertsamples <= 0 else (Nv/vertsamples) line_num = Nv/dv rospy.loginfo("Sampling %d angles, each on %d image lines" % (angle_num, line_num)) # Possibly expand the min/max height to include the camera height. hmin = min(hmin, hcam) hmax = max(hmax, hcam) rospy.loginfo("Scanning for objects %.2fmm to %.2fmm above ground" % (hmin*1000, hmax*1000)) # For testing, compute the horizon line. vhorizon = int(round(cv + fv * spitch/cpitch)) rospy.loginfo("The horizon is on row v = %d" % vhorizon) # Create the list of angle samples, and points for each angle. samplelist = setSampleList() # Create a publisher for the laser scan. pub = rospy.Publisher(SCAN_TOPIC, LaserScan, queue_size=10) # Create a subscriber to listen to the depth images. Using a # queue size of one means only the most recent message is stored # for the next subscriber callback. Skip backed-up messages. sub = rospy.Subscriber(IMAGE_TOPIC, Image, callback_image, queue_size=1, buff_size=20*Nu*Nv*2) # Proceed. rospy.logwarn("THIS IS THE SLOW PYTHON VERSION!") rospy.logwarn("caktin_make and then use the CPP version!") rospy.spin() rospy.loginfo("End convert depth image to laser scan.")