SHARE
TWEET

Untitled

a guest Nov 17th, 2019 79 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. import rospy
  3. import tf
  4. import tf.transformations as tr
  5. from std_msgs.msg import String, Header, ColorRGBA
  6. from nav_msgs.msg import OccupancyGrid, MapMetaData, Odometry
  7. from geometry_msgs.msg import Twist, PoseStamped, Point
  8. from sensor_msgs.msg import LaserScan
  9. from visualization_msgs.msg import Marker
  10. from math import sqrt, cos, sin, pi, atan2
  11. from threading import Thread, Lock
  12. from math import pi, log, exp
  13. import random
  14. import numpy as np
  15. import sys
  16.  
  17. JUNK_Q_TO_BE_REPLACED = np.array( [0,0,0,1])
  18. JUNK_P_TO_BE_REPLACED = np.array( [0,0,0])
  19. JUNK_R_TO_BE_REPLACED = np.eye(3)
  20.  
  21.  
  22. class OccupancyGridMap:
  23.     def __init__(self, num_rows, num_cols, meters_per_cell, grid_origin_in_map_frame, init_log_odds):
  24.         self.num_rows = num_rows
  25.         self.num_cols = num_cols
  26.         self.meters_per_cell = meters_per_cell
  27.         self.log_odds_ratio_occupancy_grid_map = init_log_odds * np.ones((num_rows, num_cols), dtype='float64')
  28.         self.seq = 0
  29.  
  30.         self.map_info = MapMetaData()
  31.         self.map_info.resolution = meters_per_cell
  32.         self.map_info.width = num_rows
  33.         self.map_info.height = num_cols
  34.    
  35.         self.map_info.origin.position.x = grid_origin_in_map_frame[0]
  36.         self.map_info.origin.position.y = grid_origin_in_map_frame[1]
  37.         self.map_info.origin.position.z = grid_origin_in_map_frame[2]
  38.  
  39.         self.map_info.origin.orientation.x = 0
  40.         self.map_info.origin.orientation.y = 0
  41.         self.map_info.origin.orientation.z = 0
  42.         self.map_info.origin.orientation.w = 1
  43.                
  44.     def update_log_odds_ratio_in_grid_coords(self, row, col, delta_log_odds):
  45.         assert (row >=0 and row < num_rows)
  46.         assert (col >=0 and col < num_cols)
  47.         self.log_odds_ratio_occupancy_grid_map[row][col] += delta_log_odds
  48.        
  49.     def cartesian_to_grid_coords(self, x, y):
  50.         gx = (x - self.map_info.origin.position.x) / self.map_info.resolution
  51.         gy = (y - self.map_info.origin.position.y) / self.map_info.resolution
  52.         row = min(max(int(gy), 0), self.num_rows)
  53.         col = min(max(int(gx), 0), self.num_cols)
  54.         return (row, col)
  55.  
  56.     def log_odds_ratio_to_belief(self, lor):
  57.         return 1.0 - 1.0/(1 + np.exp(lor))
  58.  
  59.     def get_map_as_ros_msg(self, timestamp, frame_id):
  60.         self.seq+= 1
  61.         msg = OccupancyGrid()
  62.         msg.header.seq = self.seq
  63.         msg.header.stamp = timestamp
  64.         msg.header.frame_id = frame_id
  65.         msg.info = self.map_info
  66.        
  67.         occupancy_belief = 100*self.log_odds_ratio_to_belief(self.log_odds_ratio_occupancy_grid_map)
  68.  
  69.         assert (occupancy_belief >= 0).all()
  70.         assert (occupancy_belief <= 100).all()
  71.        
  72.         msg.data = occupancy_belief.astype(dtype='int8', copy=True).reshape((self.num_rows*self.num_cols, ))
  73.         return msg
  74.        
  75. class HuskyMapper:
  76.     def __init__(self, num_rows, num_cols, meters_per_cell):
  77.         rospy.init_node('occupancy_grid_mapper', anonymous=True)
  78.         self.tf_listener = tf.TransformListener()
  79.  
  80.         self.odometry_position_noise_std_dev = rospy.get_param("~odometry_position_noise_std_dev")
  81.         self.odometry_orientation_noise_std_dev = rospy.get_param("~odometry_orientation_noise_std_dev")
  82.        
  83.         og_origin_in_map_frame = np.array([-20, -10, 0])
  84.         self.init_log_odds_ratio = 0 #log(0.5/0.5)
  85.         self.ogm = OccupancyGridMap(num_rows, num_cols, meters_per_cell, og_origin_in_map_frame, self.init_log_odds_ratio)
  86.  
  87.         self.max_laser_range = None
  88.         self.min_laser_range = None
  89.         self.max_laser_angle = None
  90.         self.min_laser_angle = None
  91.  
  92.         self.odometry = None
  93.        
  94.         self.q_map_baselink = None   # 4x1 quaternion from husky_1/baselink to map frame
  95.         self.R_map_baselink = None   # 3x3 rotation matrix from husky_1/baselink to map frame
  96.         self.p_map_baselink = None   # 3x1 position of husky_1/baselink in map frame
  97.  
  98.         self.q_map_baselaser = None  # 4x1 quaternion from husky_1/baselaser to map frame
  99.         self.R_map_baselaser = None  # 3x3 rotation matrix from husky_1/baselaser to map frame
  100.         self.p_map_baselaser = None  # 3x1 position of husky_1/baselaser in map frame
  101.  
  102.         self.q_baselink_baselaser = np.array([1.0, 0, 0, 0])
  103.         self.R_baselink_baselaser = tr.quaternion_matrix(self.q_baselink_baselaser)[0:3,0:3]
  104.         self.p_baselink_baselaser = np.array([0.337, 0.0, 0.308])
  105.    
  106.         self.mutex = Lock()
  107.  
  108.         self.occupancy_grid_pub = rospy.Publisher('/husky_1/occupancy_grid', OccupancyGrid, queue_size=1)
  109.         self.laser_points_marker_pub = rospy.Publisher('/husky_1/debug/laser_points', Marker, queue_size=1)
  110.         self.robot_pose_pub = rospy.Publisher('/husky_1/debug/robot_pose', PoseStamped, queue_size=1)
  111.  
  112.         self.laser_sub = rospy.Subscriber('/husky_1/scan', LaserScan, self.laser_scan_callback, queue_size=1)
  113.         self.odometry_sub = rospy.Subscriber('/husky_1/odometry/ground_truth', Odometry, self.odometry_callback, queue_size=1)
  114.        
  115.     def odometry_callback(self, msg):
  116.         self.mutex.acquire()
  117.         self.odometry = msg
  118.  
  119.         # Adds noise to the odometry position measurement according to the standard deviations specified as parameters in the launch file
  120.         # We should have used noisy measurements from the Gazebo simulator, but it is more complicated to configure
  121.         # so, we add random noise here ourselves, assuming perfect odometry from the simulator.
  122.         self.odometry.pose.pose.position.x += random.gauss(0, self.odometry_position_noise_std_dev)
  123.         self.odometry.pose.pose.position.y += random.gauss(0, self.odometry_position_noise_std_dev)
  124.        
  125.         #
  126.         # TODO: populate the quaternion from the husky_1/base_link frame to the map frame
  127.         #       based on the current odometry message. In order to know more about where
  128.         #       these frames are located on the robot, run: rosrun rviz rviz and look at the TF
  129.         #       widget. Pay attention to the following frames: husky_1/base_link which is at the center
  130.         #       of the robot's body, husky_1/base_laser, which is the frame of the laser sensor,
  131.         #       and map, which is where odometry messages are expressed in. In fact, odometry
  132.         #       messages from the Husky are transformations from husky_1/base_link to map
  133.         #
  134.    
  135.         #self.q_map_baselink = JUNK_Q_TO_BE_REPLACED
  136.         self.q_map_baselink = np.array([self.odometry.pose.pose.orientation.x, self.odometry.pose.pose.orientation.y, self.odometry.pose.pose.orientation.z, self.odometry.pose.pose.orientation.w])
  137.  
  138.         # Corrupting the quaternion with noise in yaw, because we have configured the simulator
  139.         # to return noiseless orientation measurements.
  140.         yaw_noise = random.gauss(0, self.odometry_orientation_noise_std_dev) * pi/180.0
  141.         q_truebaselink_noisybaselink = np.array([0, 0, np.sin(yaw_noise), np.cos(yaw_noise)])
  142.         self.q_map_baselink = tr.quaternion_multiply(self.q_map_baselink, q_truebaselink_noisybaselink)
  143.        
  144.        
  145.         # Computes the rotation matrix from husky_1/base_link to map
  146.         self.R_map_baselink = tr.quaternion_matrix(self.q_map_baselink)[0:3,0:3]
  147.  
  148.  
  149.         #
  150.         # TODO: populate the position of the husky_1/base_link frame in map frame
  151.         #       coordinates based on the current odometry message
  152.         #
  153.         #
  154.         #self.p_map_baselink = JUNK_P_TO_BE_REPLACED
  155.         self.p_map_baselink = np.array([self.odometry.pose.pose.position.x, self.odometry.pose.pose.position.y, self.odometry.pose.pose.position.z])
  156.  
  157.  
  158.         #
  159.         # TODO: populate the quaternion from the frame husky_1/base_laser to the map frame
  160.         #       note: you have access to the static quaternion from husky_1/base_laser to
  161.         #       husky_1/base_link  
  162.         #self.q_map_baselaser = tr.quaternion_multiply(? , ?)
  163.         #self.q_map_baselaser = tr.quaternion_multiply(JUNK_Q_TO_BE_REPLACED , JUNK_Q_TO_BE_REPLACED)
  164.         self.q_map_baselaser = tr.quaternion_multiply(self.q_map_baselink, self.q_baselink_baselaser)
  165.  
  166.         #
  167.         # TODO: populate the rotation matrix from the frame husky_1/base_laser to the map frame
  168.         #       note: you have access to the static rotation matrix from husky_1/base_laser to
  169.         #       husky_1/base_link
  170.         #       also note: np.dot(A,B) multiplies numpy matrices A and B, whereas A*B is element-wise
  171.         #       multiplication, which is not usually what you want
  172.         #
  173.         #self.R_map_baselaser = JUNK_R_TO_BE_REPLACED
  174.         self.R_map_baselaser = np.dot(self.R_map_baselink, self.R_baselink_baselaser)
  175.  
  176.  
  177.         #
  178.         # TODO: populate the origin of the frame husky_1/base_laser in coordinates of the map frame
  179.         #       note: you have access to the static rotation matrix from husky_1/base_laser to
  180.         #       husky_1/base_link and also to the origin of the husky_1/base_laser frame in coordinates of
  181.         #       frame husky_1/base_link
  182.         #       also note: np.dot(A,B) multiplies numpy matrices A and B, whereas A*B is element-wise
  183.         #       multiplication, which is not usually what you want
  184.         #
  185.         #self.p_map_baselaser =         JUNK_P_TO_BE_REPLACED
  186.         self.p_map_baselaser = self.p_map_baselink + np.dot(self.R_map_baselink, self.p_baselink_baselaser)
  187.     #self.p_map_baselaser = self.p_map_baselink + self.p_baselink_baselaser
  188.         self.mutex.release()
  189.  
  190.        
  191.     def from_laser_to_map_coordinates(self, points_in_baselaser_frame):
  192.         #
  193.         # The robot's odometry is with respect to the map frame, but the points measured from
  194.         # the laser are given with respec to the frame husky_1/base_laser. This function convert
  195.         # the measured points in the laser scan from husky_1/base_laser to the map frame.
  196.         #
  197.         points_in_map_frame = [np.dot(self.R_map_baselaser, xyz_baselaser) + self.p_map_baselaser for xyz_baselaser in points_in_baselaser_frame ]
  198.         return points_in_map_frame
  199.        
  200.  
  201.     def is_in_field_of_view(self, robot_row, robot_col, robot_theta, row, col):
  202.         # Returns true iff the cell (row, col) in the grid is in the field of view of the 2D laser of the
  203.         # robot located at cell (robot_row, robot_col) and having yaw robot_theta in the map frame.  
  204.         # Useful things to know:
  205.         # 1) self.ogm.meters_per_cell converts cell distances to metric distances
  206.         # 2) atan2(y,x) gives the angle of the vector (x,y)
  207.         # 3) atan2(sin(theta_1 - theta_2), cos(theta_1 - theta_2)) gives the angle difference between theta_1 and theta_2 in [-pi, pi]
  208.         # 4) self.max_laser_range and self.max_laser_angle specify some of the limits of the laser sensor
  209.         #
  210.         # TODO: fill logic in here
  211.         #
  212.  
  213.     # get dist
  214.         diff_x = col - robot_col
  215.         diff_y = row - robot_row
  216.         dist = sqrt((diff_y)**2 + (diff_x)**2) * self.ogm.meters_per_cell
  217.  
  218.     # get angle
  219.         vector_angle = atan2(diff_y, diff_x)
  220.         correct_vector_angle = atan2(sin(vector_angle - robot_theta), cos(vector_angle - robot_theta))
  221.  
  222.     # if cell is between min and max angle and dist is between min and max range
  223.         if (dist >= self.min_laser_range and dist <= self.max_laser_range and correct_vector_angle <= self.max_laser_angle and correct_vector_angle >= self.min_laser_angle):
  224.             return True
  225.         else:
  226.             return False
  227.  
  228.     def inverse_measurement_model(self, row, col, robot_row, robot_col, robot_theta_in_map, beam_ranges, beam_angles):
  229.         alpha = 0.1
  230.         beta = 10*pi/180.0
  231.         p_occupied = 0.999
  232.        
  233.         #
  234.         # TODO: Find the range r and angle diff_angle of the beam (robot_row, robot_col) ------> (row, col)  
  235.         # r should be in meters and diff_angle should be in [-pi, pi]. Useful things to know are same as above.
  236.         #
  237.  
  238.     # convert to meters
  239.         robot_col_meters = robot_col * self.ogm.meters_per_cell
  240.         robot_row_meters = robot_row * self.ogm.meters_per_cell
  241.  
  242.         row_meters = row * self.ogm.meters_per_cell
  243.         col_meters = col * self.ogm.meters_per_cell
  244.  
  245.     # r and diff angle
  246.         r = sqrt((row_meters - robot_row_meters)**2 + (col_meters - robot_col_meters)**2)
  247.         cell_angle = atan2(row - robot_row, col - robot_col)
  248.         diff_angle = atan2(sin(cell_angle - robot_theta_in_map), cos(cell_angle - robot_theta_in_map))
  249.  
  250.         closest_beam_angle, closest_beam_idx = min((val, idx) for (idx, val) in enumerate([ abs(diff_angle - ba) for ba in beam_angles ]))
  251.         r_cb = beam_ranges[closest_beam_idx]
  252.         theta_cb = beam_angles[closest_beam_idx]
  253.  
  254.         if r > min(self.max_laser_range, r_cb + alpha/2.0) or abs(diff_angle - theta_cb) > beta/2.0:
  255.             return self.init_log_odds_ratio
  256.        
  257.         if r_cb < self.max_laser_range and abs(r - r_cb) < alpha/2.0:
  258.             return log(p_occupied/(1-p_occupied))
  259.        
  260.         if r <= r_cb:
  261.             return log((1-p_occupied)/p_occupied)
  262.  
  263.         return 0.0
  264.  
  265.        
  266.     def laser_scan_callback(self, msg):
  267.         self.mutex.acquire()
  268.        
  269.         self.min_laser_angle = msg.angle_min
  270.         self.max_laser_angle = msg.angle_max
  271.         self.min_laser_range = msg.range_min
  272.         self.max_laser_range = msg.range_max
  273.        
  274.         if self.odometry is None:
  275.             # ignore the laser message if no odometry has been received
  276.             self.mutex.release()
  277.             return
  278.                
  279.         N = len(msg.ranges)
  280.        
  281.         ranges_in_baselaser_frame = msg.ranges
  282.         angles_in_baselaser_frame = [(msg.angle_max - msg.angle_min)*float(i)/N + msg.angle_min for i in xrange(len(msg.ranges))]
  283.         angles_in_baselink_frame = angles_in_baselaser_frame[::-1]
  284.         # This is because the z-axis of husky_1/base_laser is pointing downwards, while for husky_1/base_link and the map frame
  285.         # the z-axis points upwards
  286.  
  287.        
  288.         points_xyz_in_baselaser_frame = [np.array([r*cos(theta), r*sin(theta), 0])   for (r, theta) in zip(ranges_in_baselaser_frame, angles_in_baselaser_frame) if r < self.max_laser_range and r > self.min_laser_range]
  289.        
  290.         points_xyz_in_map_frame = self.from_laser_to_map_coordinates(points_xyz_in_baselaser_frame)
  291.        
  292.         baselaser_x_in_map = self.p_map_baselaser[0]
  293.         baselaser_y_in_map = self.p_map_baselaser[1]
  294.         baselaser_row, baselaser_col = self.ogm.cartesian_to_grid_coords(baselaser_x_in_map, baselaser_y_in_map)
  295.         _, _, yaw_map_baselaser = tr.euler_from_quaternion(self.q_map_baselaser)
  296.         _, _, yaw_map_baselink = tr.euler_from_quaternion(self.q_map_baselink)
  297.  
  298.        
  299.         # Publishing the pose of the robot as a red arrow in rviz to help you debug
  300.         ps = self._get_pose_marker(msg.header.stamp, 'map', self.p_map_baselaser, self.q_map_baselaser)
  301.  
  302.         # Publishing the points of the laser transformed into the map frame, as green points in rviz, to help you debug
  303.         pts_marker = self._get_2d_laser_points_marker(msg.header.stamp, 'map', points_xyz_in_map_frame)
  304.         self.mutex.release()
  305.  
  306.         self.robot_pose_pub.publish(ps)
  307.         self.laser_points_marker_pub.publish(pts_marker)
  308.        
  309.        
  310.         #
  311.         # This is the main loop in occupancy grid mapping
  312.         #
  313.         max_laser_range_in_cells = int(self.max_laser_range / self.ogm.meters_per_cell) + 1
  314.         for delta_row in xrange(-max_laser_range_in_cells, max_laser_range_in_cells):
  315.             for delta_col in xrange(-max_laser_range_in_cells, max_laser_range_in_cells):
  316.                 row = baselaser_row + delta_row
  317.                 col = baselaser_col + delta_col
  318.  
  319.                 if row < 0 or row >= self.ogm.num_rows or col < 0 or col >= self.ogm.num_cols:
  320.                     continue
  321.                
  322.                 if self.is_in_field_of_view(baselaser_row, baselaser_col, yaw_map_baselink, row, col):
  323.                     delta_log_odds = self.inverse_measurement_model(row,
  324.                                                                     col,
  325.                                                                     baselaser_row,
  326.                                                                     baselaser_col,
  327.                                                                     yaw_map_baselaser,
  328.                                                                     ranges_in_baselaser_frame,
  329.                                                                     angles_in_baselink_frame) - self.init_log_odds_ratio
  330.                    
  331.                     self.ogm.update_log_odds_ratio_in_grid_coords(row, col, delta_log_odds)
  332.        
  333.         self.occupancy_grid_pub.publish(self.ogm.get_map_as_ros_msg(msg.header.stamp, 'map'))
  334.    
  335.  
  336.     def _get_2d_laser_points_marker(self, timestamp, frame_id, pts_in_map):
  337.         msg = Marker()
  338.         msg.header.stamp = timestamp
  339.         msg.header.frame_id = frame_id
  340.         msg.ns = 'laser_points'
  341.         msg.id = 0
  342.         msg.type = 6
  343.         msg.action = 0
  344.         msg.points = [Point(pt[0], pt[1], pt[2]) for pt in pts_in_map]
  345.         msg.colors = [ColorRGBA(0, 1.0, 0, 1.0) for pt in pts_in_map]
  346.        
  347.         for pt in pts_in_map:
  348.             assert((not np.isnan(pt).any()) and np.isfinite(pt).all())
  349.        
  350.         msg.scale.x = 0.1
  351.         msg.scale.y = 0.1
  352.         msg.scale.z = 0.1
  353.         return msg
  354.  
  355.     def _get_pose_marker(self, timestamp, frame_id, p, q):
  356.         ps = PoseStamped()
  357.         ps.pose.position.x = p[0]
  358.         ps.pose.position.y = p[1]
  359.         ps.pose.position.z = p[2]
  360.         ps.pose.orientation.x = q[0]
  361.         ps.pose.orientation.y = q[1]
  362.         ps.pose.orientation.z = q[2]
  363.         ps.pose.orientation.w = q[3]
  364.         ps.header.stamp = timestamp
  365.         ps.header.frame_id = frame_id
  366.         return ps
  367.        
  368.     def run(self):
  369.         rate = rospy.Rate(200)
  370.         while not rospy.is_shutdown():
  371.             rate.sleep()
  372.  
  373.    
  374. if __name__ == '__main__':
  375.     num_rows = 250
  376.     num_cols = 250
  377.     meters_per_cell = 0.2
  378.     hm = HuskyMapper(num_rows, num_cols, meters_per_cell)
  379.     hm.run()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top