class WaitsForTransform(Node): """ Wait for a transform using callbacks. This class is an example of waiting for transforms. It avoids blocking the executor by registering a callback to be called when a transform becomes available. """ def __init__(self): super().__init__('example_waits_for_transform') self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self) self._output_timer = self.create_timer(1.0, self.on_timer) self._lock = threading.Lock() self._tf_future = None self._from_frame = None self._to_frame = None self._when = None def on_tf_ready(self, future): with self._lock: self._tf_future = None if future.result(): try: self._tf_buffer.lookup_transform(self._to_frame, self._from_frame, self._when) except LookupException: self.get_logger().info('transform no longer available') else: self.get_logger().info('Got transform') def on_timer(self): if self._tf_future: self.get_logger().warn('Still waiting for transform') return with self._lock: self._from_frame = 'sonar_2' self._to_frame = 'starboard_wheel' self._when = rclpy.time.Time() self._tf_future = self._tf_buffer.wait_for_transform_async( self._to_frame, self._from_frame, self._when) self._tf_future.add_done_callback(self.on_tf_ready) self.get_logger().info( 'Waiting for transform from {} to {}'.format( self._from_frame, self._to_frame))
class PeerInterfaceNode(Node): def __init__(self, robot_name): super().__init__('peer_interface_node_' + robot_name) self.e_util = ExploreUtil() self.robot_name_ = robot_name self.peer_robot_registry_list_ = dict( ) # all the active robots, including current self robot self.current_pose_local_frame_ = Pose() self.current_pose_world_frame_ = Pose() self.current_state_ = self.e_util.SYSTEM_INIT self.current_target_pose_ = Pose() #robot_registry subscriber self.robot_registry_sub_ = self.create_subscription( RobotRegistry, 'robot_registry', self.robotRegistryCallback, 10) self.robot_registry_sub_ # prevent unused variable warning #timer for getting current robot pose # self.get_current_pose_timer_ = self.create_timer(2, self.onGetCurrentPoseTimer) #service server for distributing current robot pose to peer robots self.current_pose_srv = self.create_service( GetPeerRobotPose, self.robot_name_ + '/get_current_robot_pose', self.getCurrentRobotPoseCallback) # self.robot_state_srv = self.create_service(GetPeerRobotState, self.robot_name_ + '/get_peer_robot_state', self.getPeerRobotStateCallback) # self.robot_state_pid_srv = self.create_service(GetPeerRobotStatePid, self.robot_name_ + '/get_peer_robot_state_and_pid', self.getPeerRobotStatePidCallback) timer_period = 2.5 # seconds self.timer = self.create_timer(timer_period, self.timerCallback) self.robottrack_publisher_ = self.create_publisher( RobotTracks, 'robot_tracks', 10) self.robottrack_marker_publisher_ = self.create_publisher( Marker, self.robot_name_ + '/robot_tracks_marker', 10) self.robot_tracks = [] self.last_track_ = Point() self.declare_parameters(namespace='', parameters=[('robot_name', None), ('simulation_mode', None), ('tb0_init_offset', None), ('tb1_init_offset', None), ('tb2_init_offset', None), ('tb3_init_offset', None), ('pid', None)]) self.current_robot_pid_ = self.get_parameter('pid').value self._lock = threading.Lock() self._tf_future = None self._when = None self.local_map_frame_ = self.robot_name_ + '/map' self.local_robot_frame_ = self.robot_name_ + '/base_footprint' self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self) self.cmd_vel_pub_ = self.create_publisher(Twist, robot_name + '/cmd_vel', 10) self.navigate_to_pose_client_ = ActionClient( self, NavigateToPose, robot_name + '/navigate_to_pose') self.get_path_result = None self.get_path_done_ = False self.navigate_to_pose_state_ = self.e_util.NAVIGATION_NO_GOAL self.init_offset_dict_ = dict() self.init_offset_to_current_robot_dict_ = dict() # self.getPeerRobotInitPose() self.cluster_range_limit_ = 5.5 self.current_cluster_ = [] #priority id request client self.pid_request_client_dict_ = dict() #peer robot state request client self.peer_robot_state_client_dict_ = dict() #peer robot state_and_pid request client self.peer_robot_state_and_pic_client_dict_ = dict() def publishRobotTrackMarker(self, robot_name, robot_tracks): m = Marker() m.header = Header() m.header.stamp = self.get_clock().now().to_msg() m.header.frame_id = "world" m.id = 0 m.type = 7 m.action = 0 m.scale.x = 0.5 m.scale.y = 0.5 m.scale.z = 0.5 if robot_name == 'tb0': m.color.r = 0.0 m.color.g = 1.0 m.color.b = 0.0 m.color.a = 1.0 elif robot_name == 'tb1': m.color.r = 1.0 m.color.g = 0.0 m.color.b = 0.0 m.color.a = 1.0 elif robot_name == 'tb2': m.color.r = 0.0 m.color.g = 1.0 m.color.b = 1.0 m.color.a = 1.0 elif robot_name == 'tb3': m.color.r = 0.0 m.color.g = 0.0 m.color.b = 1.0 m.color.a = 1.0 m.points = robot_tracks self.robottrack_marker_publisher_.publish(m) def timerCallback(self): msg = RobotTracks() msg.robot_name.data = self.robot_name_ if self.getRobotCurrentPose(): curr_pt = Point() curr_pt.x = self.current_pose_world_frame_.position.x curr_pt.y = self.current_pose_world_frame_.position.y curr_pt.z = 0.0 if (curr_pt.x - self.last_track_.x) * (curr_pt.x - self.last_track_.x) + ( curr_pt.y - self.last_track_.y) * ( curr_pt.y - self.last_track_.y) > 1.5 * 1.5: self.robot_tracks.append(curr_pt) msg.robot_tracks = self.robot_tracks self.robottrack_publisher_.publish(msg) self.publishRobotTrackMarker(self.robot_name_, msg.robot_tracks) self.last_track_ = curr_pt else: self.get_logger().error( "timer_callback():getRobotCurrentPose() failed, something is wrong" ) return # self.get_logger().error('Publishing {}''s RobotTracks'.format(self.robot_name_)) # self.get_logger().warn('******************************************') # self.get_logger().warn('******************************************') # self.get_logger().warn('******************************************') # self.get_logger().warn('******************************************') # self.get_logger().warn('******************************************') def getCurrentRobotPoseCallback(self, request, response): get_robot_current_pose = False while not get_robot_current_pose: if self.getRobotCurrentPose(): response.robot_pose = self.current_pose_local_frame_ get_robot_current_pose = True self.get_logger().warn('{} getCurrentRobotPoseCallback'.format( self.robot_name_)) return response def setCurrentTargetPose(self, target_pose_in_current_frame): self.current_target_pose_ = target_pose_in_current_frame def getPeerRobotStateCallback(self, request, response): get_robot_current_pose = False while not get_robot_current_pose: if self.getRobotCurrentPose(): self.current_pose_world_frame_ = self.current_pose_local_frame_ self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[ self.robot_name_].position.x self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[ self.robot_name_].position.y self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[ self.robot_name_].position.z get_robot_current_pose = True self.get_logger().warn( '{} getPeerRobotStateCallback, robot_pose:{},{}'.format( self.robot_name_, self.current_pose_world_frame_.position.x, self.current_pose_world_frame_.position.y)) response.robot_state = RobotState() response.robot_state.robot_name.data = self.robot_name_ response.robot_state.robot_pose_world_frame = self.current_pose_world_frame_ response.robot_state.battery_level = 100.0 response.robot_state.current_state = self.current_state_ response.robot_state.current_target_pose = self.current_target_pose_ response.robot_state.pid = self.current_robot_pid_ return response def getPeerRobotStatePidCallback(self, request, response): self.get_logger().warn('{} getPeerRobotStatePidCallback'.format( self.robot_name_)) response.robot_state_and_pid = RobotStatePid() response.robot_state_and_pid.robot_name.data = self.robot_name_ response.robot_state_and_pid.current_state = self.current_state_ response.robot_state_and_pid.pid = self.current_robot_pid_ return response def robotRegistryCallback(self, msg): peer_name = msg.robot_name.data # if peer_name == self.robot_name_: # return # self.get_logger().info('robot "%s" registered' % msg.robot_name.data) seconds = msg.header.stamp.sec nanoseconds = msg.header.stamp.nanosec if peer_name in self.peer_robot_registry_list_: self.peer_robot_registry_list_[ peer_name] = seconds * 1000000000 + nanoseconds else: self.peer_robot_registry_list_[ peer_name] = seconds * 1000000000 + nanoseconds self.updateRobotRegistry() def updateRobotRegistry(self): now = self.get_clock().now().nanoseconds() for robot in self.peer_robot_registry_list_: if now - self.peer_robot_registry_list_[robot] > 5000000000: del self.peer_robot_registry_list_[robot] def on_tf_ready(self, future): with self._lock: self._tf_future = None if future.result(): try: transform = self._tf_buffer.lookup_transform( self.local_map_frame_, self.local_robot_frame_, self._when) self.current_pose_local_frame_.position.x = transform.transform.translation.x self.current_pose_local_frame_.position.y = transform.transform.translation.y self.current_pose_local_frame_.position.z = transform.transform.translation.z self.current_pose_local_frame_.orientation.x = transform.transform.rotation.x self.current_pose_local_frame_.orientation.y = transform.transform.rotation.y self.current_pose_local_frame_.orientation.z = transform.transform.rotation.z self.current_pose_local_frame_.orientation.w = transform.transform.rotation.w self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[ self.robot_name_].position.x self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[ self.robot_name_].position.y self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[ self.robot_name_].position.z except LookupException: self.get_logger().info('transform no longer available') else: self.get_logger().info('Got transform') def onGetCurrentPoseTimer(self): if self._tf_future: self.get_logger().warn('Still waiting for transform') return with self._lock: # transform = self._tf_buffer.lookup_transform(self.local_map_frame_, self.local_robot_frame_, when,timeout=Duration(seconds=5.0)) self._tf_future = self._tf_buffer.wait_for_transform_async( self.local_map_frame_, self.local_robot_frame_, self._when) self._tf_future.add_done_callback(self.on_tf_ready) self.get_logger().info( 'Waiting for transform from {} to {}'.format( self.local_robot_frame_, self.local_map_frame_)) def getRobotCurrentPose(self): #return True, if success, #return False, if exception when = rclpy.time.Time() try: # Suspends callback until transform becomes available t_0 = time.time() transform = self._tf_buffer.lookup_transform( self.local_map_frame_, self.local_robot_frame_, when, timeout=Duration(seconds=5.0)) # self.get_logger().info('Got {}'.format(repr(transform))) self.current_pose_local_frame_.position.x = transform.transform.translation.x self.current_pose_local_frame_.position.y = transform.transform.translation.y self.current_pose_local_frame_.position.z = transform.transform.translation.z self.current_pose_local_frame_.orientation.x = transform.transform.rotation.x self.current_pose_local_frame_.orientation.y = transform.transform.rotation.y self.current_pose_local_frame_.orientation.z = transform.transform.rotation.z self.current_pose_local_frame_.orientation.w = transform.transform.rotation.w self.current_pose_world_frame_.position.x = self.current_pose_local_frame_.position.x + self.init_offset_dict_[ self.robot_name_].position.x self.current_pose_world_frame_.position.y = self.current_pose_local_frame_.position.y + self.init_offset_dict_[ self.robot_name_].position.y self.current_pose_world_frame_.position.z = self.current_pose_local_frame_.position.z + self.init_offset_dict_[ self.robot_name_].position.z t_1 = time.time() # self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pose_local_frame_.position.x, self.current_pose_local_frame_.position.y, t_1 - t_0)) return True except LookupException as e: self.get_logger().error('failed to get transform {}'.format( repr(e))) return False #self.init_offset_dict_ : relative position of peer robot's fixed frame to the 'world' frame, not relative to the current robot frame def getPeerRobotInitPose(self): for robot in self.peer_robot_registry_list_: param_name = robot + '_init_offset' init_offset_param = self.get_parameter(param_name) init_offset = init_offset_param.value if robot not in self.init_offset_dict_: self.init_offset_dict_[robot] = Pose() self.init_offset_dict_[robot].position.x = init_offset[0] self.init_offset_dict_[robot].position.y = init_offset[1] self.init_offset_dict_[robot].position.z = init_offset[2] self.init_offset_dict_[robot].orientation.x = init_offset[3] self.init_offset_dict_[robot].orientation.y = init_offset[4] self.init_offset_dict_[robot].orientation.z = init_offset[5] self.init_offset_dict_[robot].orientation.w = init_offset[6] current_robot_offset_world_pose = self.init_offset_dict_[ self.robot_name_] for peer in self.peer_robot_registry_list_: if peer == self.robot_name_: self.init_offset_to_current_robot_dict_[peer] = Pose() self.init_offset_to_current_robot_dict_[peer].position.x = 0.0 self.init_offset_to_current_robot_dict_[peer].position.y = 0.0 self.init_offset_to_current_robot_dict_[peer].position.z = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.x = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.y = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.z = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.w = 1.0 else: self.init_offset_to_current_robot_dict_[peer] = Pose() self.init_offset_to_current_robot_dict_[ peer].position.x = self.init_offset_dict_[ peer].position.x - current_robot_offset_world_pose.position.x self.init_offset_to_current_robot_dict_[ peer].position.y = self.init_offset_dict_[ peer].position.y - current_robot_offset_world_pose.position.y self.init_offset_to_current_robot_dict_[ peer].position.z = self.init_offset_dict_[ peer].position.z - current_robot_offset_world_pose.position.z self.init_offset_to_current_robot_dict_[ peer].orientation.x = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.y = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.z = 0.0 self.init_offset_to_current_robot_dict_[ peer].orientation.w = 1.0 #peer_list: if given, the list of robot names to request robot state information from, if None, request from self.peer_robot_registry_list_ (all the active robots) def getPeerRobotStateFunction(self, peer_list=None): service_client_dict = dict() service_response_future = dict() peer_robot_state_dict = dict() peer_robot_state_update = dict() if peer_list == None: peer_list = self.peer_robot_registry_list_ for robot in peer_list: if robot not in self.peer_robot_state_client_dict_: service_name = robot + "/get_peer_robot_state" self.peer_robot_state_client_dict_[robot] = self.create_client( GetPeerRobotState, service_name) while not self.peer_robot_state_client_dict_[ robot].wait_for_service(timeout_sec=5.0): self.get_logger().error( '/get_peer_robot_state service not available in robot {}'. format(robot)) req = GetPeerRobotState.Request() service_response_future[ robot] = self.peer_robot_state_client_dict_[robot].call_async( req) peer_robot_state_update[robot] = False t_0 = time.time() peer_robot_state_done = False while not peer_robot_state_done and time.time() - t_0 < 10.0: peer_robot_state_done = True for robot in peer_list: if peer_robot_state_update[robot] == False: # self.get_logger().info('check get_peer_robot_state response future') if service_response_future[robot].done(): response = service_response_future[robot].result() peer_robot_state_dict[robot] = response peer_robot_state_update[robot] = True else: peer_robot_state_done = False return peer_robot_state_dict def getPeerRobotPosesInLocalFrameUsingTf(self): peer_robot_pose_dict = dict() for robot in self.peer_robot_registry_list_: when = rclpy.time.Time() try: # Suspends callback until transform becomes available t_0 = time.time() transform = self._tf_buffer.lookup_transform( robot + "/map", robot + "/base_footprint", when, timeout=Duration(seconds=5.0)) # self.get_logger().info('Got {}'.format(repr(transform))) curr_robot_pose_local_frame = Pose() curr_robot_pose_local_frame.position.x = transform.transform.translation.x curr_robot_pose_local_frame.position.y = transform.transform.translation.y curr_robot_pose_local_frame.position.z = transform.transform.translation.z curr_robot_pose_local_frame.orientation.x = transform.transform.rotation.x curr_robot_pose_local_frame.orientation.y = transform.transform.rotation.y curr_robot_pose_local_frame.orientation.z = transform.transform.rotation.z curr_robot_pose_local_frame.orientation.w = transform.transform.rotation.w t_1 = time.time() #self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(curr_robot_pose_local_frame.position.x, curr_robot_pose_local_frame.position.y, t_1 - t_0)) peer_robot_pose_dict[robot] = curr_robot_pose_local_frame except LookupException as e: self.get_logger().error( 'failed to get transform for robot {},{}'.format( robot, repr(e))) return peer_robot_pose_dict def getPeerRobotPosesInLocalFrame(self): service_client_dict = dict() service_response_future = dict() peer_robot_pose_dict = dict() # always try to request and merge all the peers, no matter whether discovered at current timestep # for robot in self.robot_peers_: #self.peer_map_.clear() #self.peer_local_frontiers_.clear() for robot in self.peer_robot_registry_list_: service_name = robot + '/get_current_robot_pose' service_client_dict[robot] = self.create_client( GetPeerRobotPose, service_name) while not service_client_dict[robot].wait_for_service( timeout_sec=5.0): self.get_logger().info( '/get_current_robot_pose service not available, waiting again...' ) req = GetPeerRobotPose.Request() req.robot_name.data = robot service_response_future[robot] = service_client_dict[ robot].call_async(req) # rclpy.spin_once(self) # self.peer_data_updated_[robot] = False # self.peer_map_[robot] = service_response_future[robot].map # self.peer_local_frontiers_[robot] = service_response_future[robot].local_frontier # self.peer_data_updated_[robot] = True # response = service_response_future[robot].result() t_0 = time.time() peer_update_done = False while not peer_update_done and time.time() - t_0 < 3: peer_update_done = True for robot in self.peer_robot_registry_list_: #rclpy.spin_once(self) self.get_logger().error('check service response future') if service_response_future[robot].done(): response = service_response_future[robot].result() peer_robot_pose_dict[robot] = response.robot_pose # print(self.peer_map_) # self.peer_data_updated_[robot] = True else: peer_update_done = False return peer_robot_pose_dict #TODO: currently only supports translation between local map frame and the world frame def getPeerRobotPosesInWorldFrame(self): peer_robot_pose_local_frame_dict = self.getPeerRobotPosesInLocalFrameUsingTf( ) peer_robot_pose_world_frame_dict = dict() for robot in self.peer_robot_registry_list_: if robot in self.init_offset_dict_: init_pose = self.init_offset_dict_[robot] pose_world_frame = peer_robot_pose_local_frame_dict[robot] pose_world_frame.position.x = pose_world_frame.position.x + init_pose.position.x pose_world_frame.position.y = pose_world_frame.position.y + init_pose.position.y pose_world_frame.position.z = pose_world_frame.position.z + init_pose.position.z peer_robot_pose_world_frame_dict[robot] = pose_world_frame else: self.get_logger().error( '(getPeerRobotPosesInWorldFrame) {} not in self.init_offset_dict_' .format(robot)) return peer_robot_pose_world_frame_dict def getPeerRobotPosesInCurrentRobotFrame(self): peer_robot_pose_world_frame_dict = self.getPeerRobotPosesInWorldFrame() peer_robot_pose_current_robot_frame_dict = dict() for robot in self.peer_robot_registry_list_: if robot == self.robot_name_: peer_robot_pose_current_robot_frame_dict[robot] = Pose() peer_robot_pose_current_robot_frame_dict[ robot].position.x = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].position.y = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].position.z = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].orientation.x = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].orientation.y = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].orientation.z = 0.0 peer_robot_pose_current_robot_frame_dict[ robot].orientation.w = 1.0 else: peer_robot_pose_current_robot_frame_dict[ robot] = peer_robot_pose_world_frame_dict[robot] peer_robot_pose_current_robot_frame_dict[ robot].position.x -= peer_robot_pose_world_frame_dict[ self.robot_name_].position.x peer_robot_pose_current_robot_frame_dict[ robot].position.y -= peer_robot_pose_world_frame_dict[ self.robot_name_].position.y peer_robot_pose_current_robot_frame_dict[ robot].position.z -= peer_robot_pose_world_frame_dict[ self.robot_name_].position.z #TODO rotation not dealt with return peer_robot_pose_current_robot_frame_dict def distBetweenRobotPoses(self, pose_a, pose_b): x_a = pose_a.position.x y_a = pose_a.position.y x_b = pose_b.position.x y_b = pose_b.position.y dist = math.sqrt((x_a - x_b) * (x_a - x_b) + (y_a - y_b) * (y_a - y_b)) return dist #each robot calculates the cluster it belongs to using BFS, the neighborhood is determined based on distance, # then send its priority id among its cluster, then build consensus among the cluster, about which robot is # the leader. def getCluster(self): peer_robot_pose_world_frame_dict = self.getPeerRobotPosesInWorldFrame() #self.get_logger().info('(getCluster)after getPeerRobotPosesInWorldFrame(), size:{}'.format(len(peer_robot_pose_world_frame_dict))) bfs_queue = deque() bfs_queue.append(self.robot_name_) cluster_list = [] is_visited_map = dict() is_visited_map[self.robot_name_] = True while len(bfs_queue) > 0: curr_robot = bfs_queue[0] bfs_queue.popleft() #TODO check whether index curr_robot exists curr_pose = peer_robot_pose_world_frame_dict[curr_robot] cluster_list.append(curr_robot) for neigh in peer_robot_pose_world_frame_dict: if neigh == curr_robot or neigh in is_visited_map: continue neigh_pose = peer_robot_pose_world_frame_dict[neigh] dist = self.distBetweenRobotPoses(curr_pose, neigh_pose) if dist < self.cluster_range_limit_: is_visited_map[neigh] = True bfs_queue.append(neigh) self.current_cluster_ = copy.deepcopy(cluster_list) return cluster_list def getClusterAndPoses(self): cluster_pose_dict = dict() peer_robot_pose_local_frame_dict = self.getPeerRobotPosesInCurrentRobotFrame( ) # self.get_logger().info('(getCluster)after getPeerRobotPosesInWorldFrame(), size:{}'.format(len(peer_robot_pose_local_frame_dict))) bfs_queue = deque() bfs_queue.append(self.robot_name_) cluster_list = [] is_visited_map = dict() is_visited_map[self.robot_name_] = True while len(bfs_queue) > 0: curr_robot = bfs_queue[0] bfs_queue.popleft() #TODO check whether index curr_robot exists curr_pose = peer_robot_pose_local_frame_dict[curr_robot] cluster_list.append(curr_robot) for neigh in peer_robot_pose_local_frame_dict: if neigh == curr_robot or neigh in is_visited_map: continue neigh_pose = peer_robot_pose_local_frame_dict[neigh] dist = self.distBetweenRobotPoses(curr_pose, neigh_pose) if dist < self.cluster_range_limit_: is_visited_map[neigh] = True bfs_queue.append(neigh) self.current_cluster_ = copy.deepcopy(cluster_list) for c in cluster_list: cluster_pose_dict[c] = peer_robot_pose_local_frame_dict[c] return cluster_list, cluster_pose_dict def getPeerStatePid(self, peer_list=None): service_client_dict = dict() service_response_future = dict() peer_robot_state_pid_dict = dict() peer_robot_state_update = dict() if peer_list == None: peer_list = self.peer_robot_registry_list_ for robot in peer_list: if robot not in self.peer_robot_state_and_pic_client_dict_: service_name = robot + "/get_peer_robot_state_and_pid" self.peer_robot_state_and_pic_client_dict_[ robot] = self.create_client(GetPeerRobotStatePid, service_name) while not self.peer_robot_state_and_pic_client_dict_[ robot].wait_for_service(timeout_sec=5.0): self.get_logger().error( '/get_peer_robot_state_and_pid service not available in robot {}' .format(robot)) req = GetPeerRobotStatePid.Request() service_response_future[ robot] = self.peer_robot_state_and_pic_client_dict_[ robot].call_async(req) peer_robot_state_update[robot] = False t_0 = time.time() peer_robot_state_pid_done = False while not peer_robot_state_pid_done and time.time() - t_0 < 10.0: peer_robot_state_pid_done = True for robot in peer_list: if peer_robot_state_update[robot] == False: # self.get_logger().info('check get_peer_robot_state response future') if service_response_future[robot].done(): response = service_response_future[robot].result() peer_robot_state_pid_dict[robot] = response peer_robot_state_update[robot] = True else: peer_robot_state_pid_done = False return peer_robot_state_pid_dict #request other robots' priority id in the same cluster, if any priority id is greater then current robot's priority id, then current robot is not leader, otherwise current robot is leader def amILeaderAmongCluster(self, cluster_list): service_response_future = dict() peer_robot_pid = dict() for robot in cluster_list: if robot not in self.pid_request_client_dict_: service_name = robot + "/get_peer_robot_pid" self.pid_request_client_dict_[robot] = self.create_client( GetPeerRobotPid, service_name) while not self.pid_request_client_dict_[robot].wait_for_service( timeout_sec=5.0): self.get_logger().error( '/get_peer_robot_pid service not available in robot {}'. format(robot)) req = GetPeerRobotPid.Request() service_response_future[robot] = self.pid_request_client_dict_[ robot].call_async(req) t_0 = time.time() peer_pid_done = False while not peer_pid_done and time.time() - t_0 < 3.0: peer_pid_done = True for robot in self.current_cluster_: rclpy.spin_once(self) self.get_logger().info('check pid service response future') if service_response_future[robot].done(): response = service_response_future[robot].result() peer_robot_pid[robot] = response.pid else: peer_pid_done = False for robot in peer_robot_pid: if robot == self.robot_name_: continue if peer_robot_pid[robot] > self.current_robot_pid_: return False return True def assignTargetForCluster(self): pass def requestControlPermissionForCluster(self): pass def debugPrint(self): print('PeerInterfaceNode: debug') def navigateToPose(self, target_pose): self.navigate_to_pose_state_ = self.e_util.NAVIGATION_MOVING target_pose_stamped = PoseStamped() target_pose_stamped.pose = target_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = target_pose_stamped self.navigate_to_pose_client_.wait_for_server() # send_goal_async test self.send_goal_future = self.navigate_to_pose_client_.send_goal_async( goal_msg) self.send_goal_future.add_done_callback( self.navigateToPoseResponseCallback) return self.send_goal_future def navigateToPoseResponseCallback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info('NavigateToPoseGoal rejected') return self.get_logger().info('NavigateToPoseGoal accepted') self.get_result_future = goal_handle.get_result_async() self.get_result_future.add_done_callback( self.navigateToPoseResultCallback) def navigateToPoseResultCallback(self, future): status = future.result().status result = future.result().result if status == GoalStatus.STATUS_SUCCEEDED: # self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence)) self.get_logger().info('navigateToPoseAction finished!') self.navigate_to_pose_state_ = self.e_util.NAVIGATION_DONE else: self.get_logger().info( 'navigateToPoseAction failed with status: {0}'.format(status)) self.navigate_to_pose_state_ = self.e_util.NAVIGATION_FAILED def sendTargetForPeer(self, peer, target_pose_peer_frame): service_name = peer + '/set_robot_target_pose' set_peer_robot_target_client = self.create_client( SetRobotTargetPose, service_name) while not set_peer_robot_target_client.wait_for_service( timeout_sec=5.0): self.get_logger().error( '/set_robot_target_pose service not available in robot {}'. format(peer)) req = SetRobotTargetPose.Request() req.request_robot_name.data = self.robot_name_ req.target_pose = target_pose_peer_frame set_peer_robot_target_future = set_peer_robot_target_client.call_async( req) self.get_logger().error('Send COMMAND to {} from {}'.format( peer, self.robot_name_)) t_0 = time.time() send_peer_target_done = False while not send_peer_target_done and time.time() - t_0 < 10.0: send_peer_target_done = True if set_peer_robot_target_future.done(): response = set_peer_robot_target_future.result() self.get_logger().error( 'got response, successfully set_robot_target_pose for robot {}' .format(peer)) else: send_peer_target_done = False