订阅者不能接收到第二次的数据

[1342275724]

为什么我的节点 只能接收到一次数据 第二次就接收不到了 请大神帮我解决一下!以下是我的代码

import rclpy
from rclpy.node import Node
from nav2_simple_commander.robot_navigator import BasicNavigator,TaskResult
from geometry_msgs.msg import PolygonStamped
from demeter_adas_path_controller.utils.gps_utils import latLonYaw2Geopose
from robot_localization.srv import FromLL
import math
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from geometry_msgs.msg import PoseStamped


class PathFollowerCommander(Node):
    def __init__(self, name):
        super().__init__(name)

        # 初始化导航器和回调组
        self.navigator = BasicNavigator("basic_navigator")
        self.client_cb_group = MutuallyExclusiveCallbackGroup()
        self.sub_cb_group = MutuallyExclusiveCallbackGroup()

        # 订阅地图点数据
        self.mapviz_wp_sub = self.create_subscription(
            PolygonStamped, "/way_point_list_data", self.mapviz_wp_cb, 100, callback_group=self.sub_cb_group)

        # 创建 FromLL 服务客户端
        self.localizer = self.create_client(FromLL, '/fromLL', callback_group=self.client_cb_group)
        while not self.localizer.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service not available, waiting again...')

        self.get_logger().info('PathFollowerCommander Node Initialized Successfully.')

    def mapviz_wp_cb(self, msg: PolygonStamped):
        self.get_logger().info('Receive Points Data Successfully!')
        self.navigator.waitUntilNav2Active(localizer='controller_server')

        if msg.header.frame_id != "wgs84":
            self.get_logger().warning(
                "Received point from mapviz that is not in wgs84 frame. This is not a gps point and won't be followed")
            return

        gepose_wps = []
        points = msg.polygon.points
        yaw = 0
        for i, point in enumerate(points):
            latitude, longitude = point.y, point.x
            if i < len(points) - 1:  # 如果不是最后一个点
                next_point = points[i + 1]
                delta_x = next_point.x - point.x
                delta_y = next_point.y - point.y

                # 计算朝向的角度（在平面上）
                yaw = math.atan2(delta_y,delta_x)

            gepose_wps.append(latLonYaw2Geopose(latitude, longitude, yaw))

        wpl = []
        goals = []
        for wp in gepose_wps:
            self.req = FromLL.Request()
            self.req.ll_point.longitude = wp.position.longitude
            self.req.ll_point.latitude = wp.position.latitude
            self.req.ll_point.altitude = wp.position.altitude
            log = 'lon={:f}, lat={:f}, alt={:f}'.format(self.req.ll_point.longitude, self.req.ll_point.latitude,
                                                       self.req.ll_point.altitude)
            self.get_logger().info(log)

            # 发送请求
            future = self.localizer.call_async(self.req)
            rclpy.spin_until_future_complete(self, future)

            if future.result() is not None:
                self.resp = PoseStamped()
                self.resp.header.frame_id = 'map'
                self.resp.header.stamp = self.get_clock().now().to_msg()
                self.resp.pose.position = future.result().map_point

                log = 'x={:f}, y={:f}, z={:f}'.format(future.result().map_point.x, future.result().map_point.y, future.result().map_point.z)
                self.get_logger().info(log)
                
                self.resp.pose.orientation = wp.orientation
                wpl += [self.resp]
                goals += [self.resp]

            else:
                self.get_logger().error('Service call failed.')

        # 发送目标点列表给导航器
        # self.navigator.goThroughWaypoints(goals)
        self.get_logger().info("Start Follow Path!")
        initial_pose = PoseStamped()
        initial_pose = goals[0]
        self.navigator.setInitialPose(initial_pose)

        result_path = [x for i, x in enumerate(goals) if i % 2 != 0]
        path = self.navigator.getPathThroughPoses(initial_pose, goals=result_path)
        smoothed_path = self.navigator.smoothPath(path)
        self.navigator.followPath(smoothed_path)
        self.get_logger().info("Follow Path Completed Successfully!")

        i = 0
        while not self.navigator.isTaskComplete():
            i += 1
            feedback = self.navigator.getFeedback()
            if feedback and i % 5 == 0:
                self.get_logger().info('Estimated distance remaining to goal position: ' +
                    '{0:.3f}'.format(feedback.distance_to_goal) +
                    '\nCurrent speed of the robot: ' +
                    '{0:.3f}'.format(feedback.speed))

        result = self.navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
        elif result == TaskResult.CANCELED:
            self.get_logger().info('Goal was canceled!')
        elif result == TaskResult.FAILED:
            self.get_logger().info('Goal failed!')
        else:
            self.get_logger().info('Goal has an invalid return status!')
        self.get_logger().info('return')

def main():
    rclpy.init()

    executor = rclpy.executors.MultiThreadedExecutor()
    lc_node = PathFollowerCommander("gps_path_follower")
    executor.add_node(lc_node)
   
    try:
        executor.spin()
        # rclpy.spin(lc_node)
    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
        pass
    finally:
        lc_node.destroy_node()
        # lc_node.shutdown()


if __name__ == "__main__":
    main()

我已经确保了第一次的回调已经成功结束,已经输出return,所以这里应该没有异步阻塞的问题