Skip to content

Commit

Permalink
Improved comments.
Browse files Browse the repository at this point in the history
  • Loading branch information
Yen Shih Cheng committed Jan 20, 2020
1 parent 02036ff commit da0f800
Showing 1 changed file with 8 additions and 0 deletions.
8 changes: 8 additions & 0 deletions occupancy2.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,22 +28,30 @@ def callback(msg, tfBuffer):
rospy.loginfo('Width: %i Height: %i',msg.info.width,msg.info.height)
rospy.loginfo('Unmapped: %i Unoccupied: %i Occupied: %i Total: %i', occ_counts[0][0], occ_counts[0][1], occ_counts[0][2], total_bins)

# find transform to convert map coordinates to base_link coordinates
# lookup_transform(target_frame, source_frame, time)
trans = tfBuffer.lookup_transform('base_link', 'map', rospy.Time(0))
rospy.loginfo(['Trans: ' + str(trans.transform.translation)])
rospy.loginfo(['Rot: ' + str(trans.transform.rotation)])
# convert quaternion to Euler angles
orientation_list = [trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
rospy.loginfo(['Yaw: R: ' + str(yaw) + ' D: ' + str(np.degrees(yaw))])

# make occdata go from 0 instead of -1, reshape into 2D
oc2 = occdata + 1
# set all values above 1 (i.e. above 0 in the original map data, representing occupied locations)
oc3 = (oc2>1).choose(oc2,2)
# reshape to 2D array using column order
odata = np.uint8(oc3.reshape(msg.info.width,msg.info.height,order='F'))
# create image from 2D array using PIL
img = Image.fromarray(odata)
# rotate by 180 degrees to invert map so that the forward direction is at the top of the image
rotated = img.rotate(np.degrees(yaw)+180)
# show image using grayscale map
plt.imshow(rotated,cmap='gray')
plt.draw_all()
# pause to make sure the plot gets created
plt.pause(0.00000000001)


Expand Down

0 comments on commit da0f800

Please sign in to comment.