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