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
def state_update(state_msg): global ball_pose global goal_post_1_pose global goal_post_2_pose global tf_buffer global cam_info global ball_pub global goal_posts_pub if not cam_info: return ball_indices = [] for i, name in enumerate(state_msg.name): if name == "teensize_ball": ball_indices.append(i) post_index1 = 0 post_index2 = 0 for name in state_msg.name: if name == "teensize_goal": if post_index1 == 0: post_index1 = post_index2 else: break post_index2 += 1 ball_msgs = [] for ball_index in ball_indices: ball_pose = state_msg.pose[ball_index] ball_pose_stamped = PoseStamped() ball_pose_stamped.header.stamp = rospy.Time.now() ball_pose_stamped.header.frame_id = "map" ball_pose_stamped.pose = ball_pose ball_in_camera_optical_frame = tf_buffer.transform( ball_pose_stamped, cam_info.header.frame_id, timeout=rospy.Duration(0.5)) # we only have to compute if the ball is inside the image, if the ball is in front of the camera if ball_in_camera_optical_frame.pose.position.z >= 0: p = [ ball_in_camera_optical_frame.pose.position.x, ball_in_camera_optical_frame.pose.position.y, ball_in_camera_optical_frame.pose.position.z ] k = np.reshape(cam_info.K, (3, 3)) p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) # make sure that the transformed pixel is inside the resolution and positive. # also z has to be positive to make sure that the ball is in front of the camera and not in the back if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[ 1] <= cam_info.height: ball_in_footprint_frame = tf_buffer.transform( ball_in_camera_optical_frame, "base_footprint", timeout=rospy.Duration(0.5)) ball_relative = PoseWithCertainty() ball_relative.pose.pose = ball_in_footprint_frame.pose.position ball_relative.confidence = 1.0 ball_msgs.append(ball_relative) balls_relative_msg = PoseWithCertaintyArray() balls_relative_msg.header = ball_in_footprint_frame.header balls_relative_msg.poses = ball_msgs ball_pub.publish(balls_relative_msg) goal = PoseWithCertaintyArray() goal_post_1_pose = state_msg.pose[post_index1] goal_post_2_pose = state_msg.pose[post_index2] for gp in (goal_post_1_pose, goal_post_2_pose): goal_post_stamped = PoseWithCertainty() goal_post_stamped.pose.pose = gp left_post = deepcopy(goal_post_stamped) left_post.pose.pose.position.y += 1.35 left_post_in_camera_optical_frame = tf_buffer.transform( left_post.pose, cam_info.header.frame_id, timeout=rospy.Duration(0.5)) if left_post_in_camera_optical_frame.pose.position.z >= 0: p = [ left_post_in_camera_optical_frame.pose.position.x, left_post_in_camera_optical_frame.pose.position.y, left_post_in_camera_optical_frame.pose.position.z ] k = np.reshape(cam_info.K, (3, 3)) p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[ 1] <= cam_info.height: left_post = PoseWithCertainty() left_post.confidence = 1 left_post.pose = tf_buffer.transform( left_post.pose, "base_footprint", timeout=rospy.Duration(0.5)).pose.position goal.poses.append(left_post) right_post = goal_post_stamped right_post.pose.position.y -= 1.35 right_post_in_camera_optical_frame = tf_buffer.transform( right_post, cam_info.header.frame_id, timeout=rospy.Duration(0.5)) if right_post_in_camera_optical_frame.pose.position.z >= 0: p = [ right_post_in_camera_optical_frame.pose.position.x, right_post_in_camera_optical_frame.pose.position.y, right_post_in_camera_optical_frame.pose.position.z ] k = np.reshape(cam_info.K, (3, 3)) p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) if 0 < p_pixel[0] <= cam_info.width and 0 < p_pixel[ 1] <= cam_info.height: right_post = PoseWithCertainty() right_post.confidence = 1 right_post.pose = tf_buffer.transform( right_post_in_camera_optical_frame, "base_footprint", timeout=rospy.Duration(0.5)).pose.position goal.poses.append(right_post) if len(goal.poses) > 0: goal_posts_pub.publish(goal)