def ScanMap(self): if self.odom is None: return # Transform: odom frame -> map frame poseStamped = PoseStamped() poseStamped.header = self.odom.header poseStamped.pose = self.odom.pose.pose try: poseStamped = self.tfBuffer.transform(poseStamped, self.map_frame, timeout=rospy.Duration(1.0 / self.rate)) except: # rospy.logerr('Transform failed') return # Scan yaw = tf.transformations.euler_from_quaternion([poseStamped.pose.orientation.x, poseStamped.pose.orientation.y, poseStamped.pose.orientation.z, poseStamped.pose.orientation.w])[2] R = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) T = np.array([poseStamped.pose.position.x, poseStamped.pose.position.y]) scan_buffer = (np.matmul(self.scan_buffer, R.T) + T) scan_buffer = self.occGridParam.map2ImageTransform(scan_buffer) scan_buffer = np.int32(np.round(scan_buffer)) scan_buffer[:, :, 0] = np.clip(scan_buffer[:, :, 0], 0, self.width - 1) scan_buffer[:, :, 1] = np.clip(scan_buffer[:, :, 1], 0, self.height - 1) pixel_values = self.occMap[scan_buffer[:, :, 1], scan_buffer[:, :, 0]] mask = pixel_values > 0 indices = np.where(mask.any(axis=1), mask.argmax(axis=1), -1) pointclouds = [] for index, scan_points in zip(indices, scan_buffer): if index == -1: pointcloud = [np.inf, np.inf, np.inf] else: pointcloud = [scan_points[index, 0], scan_points[index, 1], 0.0] pointclouds.append(pointcloud) pointclouds = np.asarray(pointclouds) pointclouds[:, :2] = self.occGridParam.image2MapTransform(pointclouds[:, :2]) self.map_pointclouds = pointclouds
def ObstacleCallback(self, msg): if self.odom is None: return # Transform poseStamped = PoseStamped() poseStamped.header = msg.header poseStamped.pose = msg.pose.pose try: poseStamped = self.tfBuffer.transform(poseStamped, self.odom_frame, timeout=rospy.Duration(1.0 / self.rate)) except: # rospy.logerr('Transform failed') return x, y, z = poseStamped.pose.position.x, poseStamped.pose.position.y, poseStamped.pose.position.z dx, dy = x - self.odom.pose.pose.position.x, y - self.odom.pose.pose.position.y dist = np.sqrt(dx ** 2 + dy ** 2) if dist < self.min_range or dist > self.max_range: self.obstacle_pointclouds = None return pointclouds = np.array([[x, y, z]]) self.obstacle_pointclouds = pointclouds