From 67d533dc14febabfc9c735f925165c1802e2d0e3 Mon Sep 17 00:00:00 2001 From: Dankev55 <33918941+Dankev55@users.noreply.github.com> Date: Sat, 6 Apr 2019 16:05:57 -0500 Subject: [PATCH] WIP #7: Updated Field_Mapper Node to work with rotated transforms Rotates the local map so it fits into the field map properly. Removed loops to work with numpy more efficiently. We need to find a way to deal with the additional junk values that arise in the area surrounding the rotated map. --- robot_slam/obstacle_avoidance/Field_Mapper.py | 66 ++++++++++++++----- 1 file changed, 49 insertions(+), 17 deletions(-) diff --git a/robot_slam/obstacle_avoidance/Field_Mapper.py b/robot_slam/obstacle_avoidance/Field_Mapper.py index 01d649b..5f5b3e5 100644 --- a/robot_slam/obstacle_avoidance/Field_Mapper.py +++ b/robot_slam/obstacle_avoidance/Field_Mapper.py @@ -2,41 +2,73 @@ import rospy import numpy as np import tf2_ros +from geometry_msgs.msg import Quaternion +from tf.transformations import euler_from_quaternion +import cv2 -field = Field(length = 600, width = 400) -#listener = tf.transform_listener() -tfBuffer = tf2_ros.Buffer() -listener = tf2_ros.TransformListener(tfBuffer) -rospy.Subscriber("elevation_mapping/occ_grid", OccupancyGrid, field.update_field) -rospy.spin() class Field: def __init__(self, length, width): - self.map = np.array(size=(600,400)) #length, width + self.map = np.array(size=(600,400)) self.xpos = 0 self.ypos = 0 + self.tfBuffer = tf2_ros.Buffer() + self.listener = tf2_ros.TransformListener(tfBuffer) #self.map_pub = rospy.Publisher def update_field(self, new_map_msg): #convert msg to numpy array #get pose of robot relative to world #update corresponding region of self.map - local_map = np.reshape(OccupancyGrid.data, (OccupancyGrid.info.height, OccupancyGrid.info.width)) + + + local_map = np.reshape(new_map_msg.data, (new_map_msg.info.height, new_map_msg.info.width)) cur_tf = tfBuffer.lookup_transform("world_pose", "robot_pose", rospy.time(0)) + q = [cur_tf.transform.rotation.x, + cur_tf.transform.rotation.y, + cur_tf.transform.rotation.z, + cur_tf.transform.rotation.w] #quaternion from tf + rpy = euler_from_quaternion(Quaternion(q[0], q[1], q[2], q[3])) + #Rotate the local map based on the difference in angles of the transforms + (h, w) = local_map.shape[:2] + (origin_X, origin_Y) = (w/2, h/2) + + + M = cv2.getRotationMatrix2D((cX, cY), rpy[2], 1.0) + cos = np.abs(M[0, 0]) + sin = np.abs(M[0, 1]) + + new_w = int((h * sin) + (w * cos)) + new_h = int((h * cos) + (w * sin)) + + M[0, 2] += (new_w/2) - origin_X + M[1, 2] += (new_h/2) - origin_Y + adj_lmap = cv2.warpAffine(local_map, M, (new_w, new_h)) + + #Add values from adjusted local map to world map + + (adj_x, adj_y) = adj_lmap.shape[:2] + + local_x = cur_tf.getOrigin().x() local_y = cur_tf.getOrigin().y() - for y in local_map: - local_y = cur_tf.getOrigin().y() - for x in y: - self.map[local_y][local_x] = local_map[y][x] - local_y += 1 - local_x += 1 + (field_x, field_y) = Field.shape[:2] + field_x /= 2 + field_y /= 2 + dif_x = np.abs(field_x - local_x) + dif_y = np.abs(field_y - local_y) + + Field[dif_x:(dif_x+adj_x), dif_y:(dif_y+adj_y)] = adj_lmap + - + - - def msg_callback(new_map.msg): +if __name__ == "__main__": + field = Field(length = 600, width = 400) + #listener = tf.transform_listener() + rospy.Subscriber("elevation_mapping/occ_grid", OccupancyGrid, field.update_field) + rospy.spin()