class BlockingWaitsForTransform(Node):
    """
    Wait for a transform syncronously.

    This class is an example of waiting for transforms.
    This will block the executor if used within a callback.
    Coroutine callbacks should be used instead to avoid this.
    See :doc:`examples_tf2_py/async_waits_for_transform.py` for an example.
    """
    def __init__(self):
        super().__init__('example_blocking_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)

    def on_timer(self):
        from_frame = 'sonar_2'
        to_frame = 'starboard_wheel'
        self.get_logger().info('Waiting for transform from {} to {}'.format(
            from_frame, to_frame))
        try:
            # Block until the transform is available
            when = rclpy.time.Time()
            self._tf_buffer.lookup_transform(to_frame,
                                             from_frame,
                                             when,
                                             timeout=Duration(seconds=5.0))
        except LookupException:
            self.get_logger().info('transform not ready')
        else:
            self.get_logger().info('Got transform')
예제 #2
0
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))
예제 #3
0
class SelfPoseListener(Node):
    def __init__(self):
        super().__init__("self_pose_listener")
        self.tf_buffer = Buffer()
        self._tf_listener = TransformListener(self.tf_buffer, self)
        self.self_pose = PoseStamped()

    def get_current_pose(self):
        try:
            tf = self.tf_buffer.lookup_transform("map", "base_link",
                                                 rclpy.time.Time())
            tf_time = self.tf_buffer.get_latest_common_time("map", "base_link")
            self.self_pose = SelfPoseListener.create_pose(tf_time, "map", tf)
        except LookupException as e:
            self.get_logger().warn(
                "Required transformation not found: `{}`".format(str(e)))
            return None

    @staticmethod
    def create_pose(time, frame_id, tf):
        pose = PoseStamped()

        pose.header.stamp = time.to_msg()
        pose.header.frame_id = frame_id

        pose.pose.position.x = tf.transform.translation.x
        pose.pose.position.y = tf.transform.translation.y
        pose.pose.position.z = tf.transform.translation.z
        pose.pose.orientation.x = tf.transform.rotation.x
        pose.pose.orientation.y = tf.transform.rotation.y
        pose.pose.orientation.z = tf.transform.rotation.z
        pose.pose.orientation.w = tf.transform.rotation.w

        return pose
예제 #4
0
class MinimalSubscriber(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            Trajectory, '/planning/scenario_planning/trajectory',
            self.trajectory_callback, 10)

        self.subscription  # prevent unused variable warning

        self.goal_pose = None

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.ego_poes = None
        #self.timer = self.create_timer(0.01, self.on_timer)

    def get_ego(self):
        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform('map', 'base_link', now)
        except TransformException as ex:
            print("cannot transform")
            return
        return trans.transform.translation

    def trajectory_callback(self, msg: Trajectory):
        goal_pos = msg.points[0].pose.position
        ego_pos = self.get_ego()
        dist = sqrt((goal_pos.x - ego_pos.x)**2 + (goal_pos.y - ego_pos.y)**2)
        print(dist)
예제 #5
0
    def test_can_transform_valid_transform(self):
        buffer = Buffer()
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        self.assertEqual(buffer.set_transform(transform, 'unittest'), None)

        self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)

        output = buffer.lookup_transform('foo', 'bar', rclpy_time)
        self.assertEqual(transform.child_frame_id, output.child_frame_id)
        self.assertEqual(transform.transform.translation.x, output.transform.translation.x)
        self.assertEqual(transform.transform.translation.y, output.transform.translation.y)
        self.assertEqual(transform.transform.translation.z, output.transform.translation.z)
예제 #6
0
    def test_buffer_non_default_cache(self):
        buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=10.0))
        clock = rclpy.clock.Clock()
        rclpy_time = clock.now()
        transform = self.build_transform('foo', 'bar', rclpy_time)

        self.assertEqual(buffer.set_transform(transform, 'unittest'), None)

        self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True)

        output = buffer.lookup_transform('foo', 'bar', rclpy_time)
        self.assertEqual(transform.child_frame_id, output.child_frame_id)
        self.assertEqual(transform.transform.translation.x,
                         output.transform.translation.x)
        self.assertEqual(transform.transform.translation.y,
                         output.transform.translation.y)
        self.assertEqual(transform.transform.translation.z,
                         output.transform.translation.z)
class Tf2PoseNode(Node):
    def __init__(self, options):
        super().__init__("tf2pose")

        self._options = options
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self._pub_pose = self.create_publisher(
            PoseStamped, "/autoware_debug_tools/tf2pose/pose", 1)
        self.timer = self.create_timer((1.0 / self._options.hz),
                                       self._on_timer)

    def _on_timer(self):
        try:
            tf = self.tf_buffer.lookup_transform(self._options.tf_from,
                                                 self._options.tf_to,
                                                 rclpy.time.Time())
            time = self.tf_buffer.get_latest_common_time(
                self._options.tf_from, self._options.tf_to)
            pose = Tf2PoseNode.create_pose(time, self._options.tf_from, tf)
            self._pub_pose.publish(pose)
        except LookupException as e:
            print(e)

    @staticmethod
    def create_pose(time, frame_id, tf):
        pose = PoseStamped()

        pose.header.stamp = time.to_msg()
        pose.header.frame_id = frame_id

        pose.pose.position.x = tf.transform.translation.x
        pose.pose.position.y = tf.transform.translation.y
        pose.pose.position.z = tf.transform.translation.z
        pose.pose.orientation.x = tf.transform.rotation.x
        pose.pose.orientation.y = tf.transform.rotation.y
        pose.pose.orientation.z = tf.transform.rotation.z
        pose.pose.orientation.w = tf.transform.rotation.w

        return pose
예제 #8
0
class Robot(Node):
    def __init__(self, node_name: str):
        super().__init__(node_name)
        self.get_logger().info("initializing AutoMapper")
        self.call = None
        self.create_timer(1, self.get_map)
        self.map_data = None
        self.width = 0
        self.height = 0
        self.map_origin = Point()
        self.resolution = 0
        self.robot_yaw = 0
        self.robot_pose = Point()
        self.tf_buffer = Buffer()
        self.tf_timer = None
        self.listener = TransformListener(self.tf_buffer, self)
        try:
            self.map_srv = self.create_client(GetMap, "/map_server/map")
            self.get_logger().info("Service client created")
            while not self.map_srv.wait_for_service(timeout_sec=1.0):
                self.get_logger().info(
                    'service not available, waiting again...')
        except Exception as e:
            self.get_logger().info(e)

    @staticmethod
    def quaternion_to_euler(quaternion):
        t0 = +2.0 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)
        t1 = +1.0 - 2.0 * (quaternion.x * quaternion.x +
                           quaternion.y * quaternion.y)
        roll = atan2(t0, t1)
        t2 = +2.0 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        pitch = asin(t2)
        t3 = +2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y)
        t4 = +1.0 - 2.0 * (quaternion.y * quaternion.y +
                           quaternion.z * quaternion.z)
        yaw = atan2(t3, t4)
        return [yaw, pitch, roll]

    @staticmethod
    def euler_to_quaternion(roll, pitch, yaw):
        qx = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(
            roll / 2) * sin(pitch / 2) * sin(yaw / 2)
        qy = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(
            roll / 2) * cos(pitch / 2) * sin(yaw / 2)
        qz = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(
            roll / 2) * sin(pitch / 2) * cos(yaw / 2)
        qw = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + sin(
            roll / 2) * sin(pitch / 2) * sin(yaw / 2)
        return [qx, qy, qz, qw]

    def get_robot_pose(self):
        try:
            ((robot_x, robot_y, z),
             robot_rot) = self.tf_buffer.lookup_transform(
                 "/map", "base_link", Time())
            robot_yaw = self.quaternion_to_euler(robot_rot)[2]
            self.robot_pose = {'x': robot_x, 'y': robot_y, 'yaw': robot_yaw}
        except LookupException:
            self.get_logger().info('transform not ready')

    def get_map(self):
        self.get_logger().info("Requesting GetMap data")
        self.call = self.map_srv.call(GetMap.Request())
        if self.call.done():
            try:
                response: GetMap.Response = self.call.result()
                self.get_logger().info("GetMap data received")
                response_map = response.map()
                self.width = response_map.info.width
                self.height = response_map.info.height
                self.resolution = response_map.info.resolution
                origin_yaw = self.quaternion_to_euler(
                    response_map.info.origin.orientation)[2]
                origin_x = response_map.info.origin.position.x
                origin_y = response_map.info.origin.position.y
                self.map_origin = {
                    'x': origin_x,
                    'y': origin_y,
                    'yaw': origin_yaw
                }
                self.tf_timer = self.create_timer(1.0, self.get_robot_pose)
                self.map_data = list(response_map.data)
                for i in range(self.height):
                    for j in range(self.width):
                        self.map_data[i * self.width + j] = response_map.data[
                            (self.height - 1 - i) * self.width + j]
            except Exception as e:
                self.get_logger().info('Service call failed %r' % (e, ))

    def pose_to_pix(self):
        pix_x = (self.map_origin.y -
                 self.robot_pose.y) / self.resolution + self.height
        pix_y = (self.robot_pose.x - self.map_origin.x) / self.resolution
        pix_yaw = self.robot_pose.z - self.map_origin.z
        return int(pix_x), int(pix_y), pix_yaw
class KFHungarianTracker(Node):
    '''Use Kalman Fiter and Hungarian algorithm to track multiple dynamic obstacles

    Use Hungarian algorithm to match presenting obstacles with new detection and maintain a kalman filter for each obstacle.
    spawn ObstacleClass when new obstacles come and delete when they disappear for certain number of frames

    Attributes:
        obstacle_list: a list of ObstacleClass that currently present in the scene
        sec, nanosec: timing from sensor msg
        detection_sub: subscrib detection result from detection node
        tracker_obstacle_pub: publish tracking obstacles with ObstacleArray
        tracker_pose_pub: publish tracking obstacles with PoseArray, for rviz visualization
    '''

    def __init__(self):
        '''initialize attributes and setup subscriber and publisher'''

        super().__init__('kf_hungarian_node')
        self.declare_parameters(
            namespace='',
            parameters=[
                ('global_frame', "camera_link"),
                ('process_noise_cov', [2., 2., 0.5]),
                ('top_down', False),
                ('death_threshold', 3),
                ('measurement_noise_cov', [1., 1., 1.]),
                ('error_cov_post', [1., 1., 1., 10., 10., 10.]),
                ('vel_filter', [0.1, 2.0]),
                ('height_filter', [-2.0, 2.0]),
                ('cost_filter', 1.0)
            ])
        self.global_frame = self.get_parameter("global_frame")._value
        self.death_threshold = self.get_parameter("death_threshold")._value
        self.measurement_noise_cov = self.get_parameter("measurement_noise_cov")._value
        self.error_cov_post = self.get_parameter("error_cov_post")._value
        self.process_noise_cov = self.get_parameter("process_noise_cov")._value
        self.vel_filter = self.get_parameter("vel_filter")._value
        self.height_filter = self.get_parameter("height_filter")._value
        self.top_down = self.get_parameter("top_down")._value
        self.cost_filter = self.get_parameter("cost_filter")._value

        self.obstacle_list = []
        self.sec = 0
        self.nanosec = 0

        # subscribe to detector 
        self.detection_sub = self.create_subscription(
            ObstacleArray,
            'detection',
            self.callback,
            10)

        # publisher for tracking result
        self.tracker_obstacle_pub = self.create_publisher(ObstacleArray, 'tracking', 10)
        self.tracker_marker_pub = self.create_publisher(MarkerArray, 'marker', 10)

        # setup tf related
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

    def callback(self, msg):
        '''callback function for detection result'''

        # update delta time
        dt = (msg.header.stamp.sec - self.sec) + (msg.header.stamp.nanosec - self.nanosec) / 1e9
        self.sec = msg.header.stamp.sec
        self.nanosec = msg.header.stamp.nanosec

        # get detection
        detections = msg.obstacles
        num_of_detect = len(detections)
        num_of_obstacle = len(self.obstacle_list)

        # kalman predict
        for obj in self.obstacle_list:
            obj.predict(dt)

        # transform to global frame
        if self.global_frame is not None:
            try:
                trans = self.tf_buffer.lookup_transform(self.global_frame, msg.header.frame_id, rclpy.time.Time())
                msg.header.frame_id = self.global_frame
                for i in range(len(detections)):
                    # transform position (point)
                    p = PointStamped()
                    p.point = detections[i].position
                    detections[i].position = do_transform_point(p, trans).point
                    # transform velocity (vector3)
                    v = Vector3Stamped()
                    v.vector = detections[i].velocity
                    detections[i].velocity = do_transform_vector3(v, trans).vector
                    # transform size (vector3)
                    s = Vector3Stamped()
                    s.vector = detections[i].size
                    detections[i].size = do_transform_vector3(s, trans).vector

            except LookupException:
                self.get_logger().info('fail to get tf from {} to {}'.format(msg.header.frame_id, self.global_frame))
                return

        # hungarian matching
        cost = np.zeros((num_of_obstacle, num_of_detect))
        for i in range(num_of_obstacle):
            for j in range(num_of_detect):
                cost[i, j] = self.obstacle_list[i].distance(detections[j])
        obs_ind, det_ind = linear_sum_assignment(cost)

        # filter assignment according to cost
        new_obs_ind = []
        new_det_ind = []
        for o, d in zip(obs_ind, det_ind):
            if cost[o, d] < self.cost_filter:
                new_obs_ind.append(o)
                new_det_ind.append(d)
        obs_ind = new_obs_ind
        det_ind = new_det_ind

        # kalman update
        for o, d in zip(obs_ind, det_ind):
            self.obstacle_list[o].correct(detections[d])

        # birth of new detection obstacles and death of disappear obstacle
        self.birth(det_ind, num_of_detect, detections)
        dead_object_list = self.death(obs_ind, num_of_obstacle)

        # apply velocity and height filter
        filtered_obstacle_list = []
        for obs in self.obstacle_list:
            obs_vel = np.linalg.norm(np.array([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z]))
            obs_height = obs.msg.position.z
            if obs_vel > self.vel_filter[0] and obs_vel < self.vel_filter[1] and obs_height > self.height_filter[0] and obs_height < self.height_filter[1]:
                filtered_obstacle_list.append(obs)

        # construct ObstacleArray
        if self.tracker_obstacle_pub.get_subscription_count() > 0:
            obstacle_array = ObstacleArray()
            obstacle_array.header = msg.header
            track_list = []
            for obs in filtered_obstacle_list:
                # do not publish obstacles with low speed
                track_list.append(obs.msg)
            obstacle_array.obstacles = track_list
            self.tracker_obstacle_pub.publish(obstacle_array)

        # rviz visualization
        if self.tracker_marker_pub.get_subscription_count() > 0:
            marker_array = MarkerArray()
            marker_list = []
            # add current active obstacles
            for obs in filtered_obstacle_list:
                obstacle_uuid = uuid.UUID(bytes=bytes(obs.msg.uuid.uuid))
                (r, g, b) = colorsys.hsv_to_rgb(obstacle_uuid.int % 360 / 360., 1., 1.) # encode id with rgb color
                # make a cube 
                marker = Marker()
                marker.header = msg.header
                marker.ns = str(obstacle_uuid)
                marker.id = 0
                marker.type = 1 # CUBE
                marker.action = 0
                marker.color.a = 0.5
                marker.color.r = r
                marker.color.g = g
                marker.color.b = b
                marker.pose.position = obs.msg.position
                angle = np.arctan2(obs.msg.velocity.y, obs.msg.velocity.x)
                marker.pose.orientation.z = np.float(np.sin(angle / 2))
                marker.pose.orientation.w = np.float(np.cos(angle / 2))
                marker.scale = obs.msg.size
                marker_list.append(marker)
                # make an arrow
                arrow = Marker()
                arrow.header = msg.header
                arrow.ns = str(obstacle_uuid)
                arrow.id = 1 
                arrow.type = 0
                arrow.action = 0
                arrow.color.a = 1.0
                arrow.color.r = r
                arrow.color.g = g
                arrow.color.b = b
                arrow.pose.position = obs.msg.position
                arrow.pose.orientation.z = np.float(np.sin(angle / 2))
                arrow.pose.orientation.w = np.float(np.cos(angle / 2))
                arrow.scale.x = np.linalg.norm([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z])
                arrow.scale.y = 0.05
                arrow.scale.z = 0.05
                marker_list.append(arrow)
            # add dead obstacles to delete in rviz
            for uuid in dead_object_list:
                marker = Marker()
                marker.header = msg.header
                marker.ns = str(uuid)
                marker.id = 0
                marker.action = 2 # delete
                arrow = Marker()
                arrow.header = msg.header
                arrow.ns = str(uuid)
                arrow.id = 1
                arrow.action = 2
                marker_list.append(marker)
                marker_list.append(arrow)
            marker_array.markers = marker_list
            self.tracker_marker_pub.publish(marker_array)

    def birth(self, det_ind, num_of_detect, detections):
        '''generate new ObstacleClass for detections that do not match any in current obstacle list'''
        for det in range(num_of_detect):
            if det not in det_ind:
                obstacle = ObstacleClass(detections[det], self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov)
                self.obstacle_list.append(obstacle)

    def death(self, obj_ind, num_of_obstacle):
        '''count obstacles' missing frames and delete when reach threshold'''
        new_object_list = []
        dead_object_list = []
        # for previous obstacles
        for obs in range(num_of_obstacle):
            if obs not in obj_ind:
                self.obstacle_list[obs].dying += 1
            else:
                self.obstacle_list[obs].dying = 0

            if self.obstacle_list[obs].dying < self.death_threshold:
                new_object_list.append(self.obstacle_list[obs])
            else:
                obstacle_uuid = uuid.UUID(bytes=bytes(self.obstacle_list[obs].msg.uuid.uuid))
                dead_object_list.append(obstacle_uuid)
        
        # add newly born obstacles
        for obs in range(num_of_obstacle, len(self.obstacle_list)):
            new_object_list.append(self.obstacle_list[obs])

        self.obstacle_list = new_object_list
        return dead_object_list
예제 #10
0
class MultiExploreNode(Node):
    def __init__(self, robot_name, total_robot_num):
        super().__init__('multi_explore_swarm_simulation_' + robot_name)
        self.DEBUG_ = True
        self.total_robot_num_ = int(total_robot_num)
        self.para_group = ReentrantCallbackGroup()
        self.local_frontiers_ = [
        ]  #list of frontier, each is list of (double, double) in the local map frame
        self.local_frontiers_msg_ = [
        ]  #list of multi_robot_interfaces.msg.Frontier
        self.global_frontiers_ = []
        self.robot_name_ = robot_name
        self.current_state_ = 0
        self.previous_state_ = -1
        #current robot_peers
        self.robot_peers_ = []
        #all the robot_peers ever discovered in history, no matter current network status
        self.persistent_robot_peers_ = []
        #the ever growing merged global_map of current robot, will update with new peer_map and new local_map
        self.persistent_global_map_ = None
        #offset from robot_peers to the self.local_fixed_frame_, {'tb0': (x_offset, y_offset) , 'tb1':(x_offset, y_offset), ...}
        self.persistent_offset_from_peer_to_local_ = dict()
        #current local_map
        self.local_map_ = None
        self.inflated_local_map_ = None
        self.current_pos_ = (-1, -1)
        self.current_pose_local_frame_ = PoseStamped()
        self.current_target_pos_ = Pose()
        self.next_target_pos_ = Pose()

        #multi robot settings
        self.peer_map_ = dict()
        self.peer_local_frontiers_ = dict()
        self.peer_data_updated_ = dict()
        self.merge_map_frontier_timeout_ = 5

        self.merged_map_ = None
        self.merged_frontiers_ = []

        self.world_frame_ = ''
        self.local_fixed_frame_ = ''
        if self.robot_name_ == '':
            self.world_frame_ = 'map'
            self.local_fixed_frame_ = 'base_link'
        else:
            self.world_frame_ = self.robot_name_ + '/map'
            self.local_fixed_frame_ = self.robot_name_ + '/base_link'

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        # self.local_map_srv = self.create_service(GetLocalMap, self.robot_name_ + '/get_local_map', self.getLocalMapCallback)
        self.local_map_and_frontier_srv = self.create_service(
            GetLocalMapAndFrontier,
            self.robot_name_ + '/get_local_map_and_frontier',
            self.getLocalMapAndFrontierCallback)

        self.local_map_and_frontier_srv = self.create_service(
            GetLocalMapAndFrontierCompress,
            self.robot_name_ + '/get_local_map_and_frontier_compress',
            self.getLocalMapAndFrontierCompressCallback)

        # self.get_map_value_srv = self.create_service(GetPeerMapValueOnCoords, self.robot_name_ + '/get_map_value_on_coords', self.getMapValueOnCoordsCallback)

        self.get_map_value_client_map_ = dict()
        self.get_local_frontier_target_pt_client_map_ = dict()

        self.robot_pose_sub_ = self.create_subscription(
            Point, self.robot_name_ + '/robot_pose', self.robotPoseCallback,
            10)
        self.robot_pose_sub_  # prevent unused variable warning

        self.wfd_service_client = self.create_client(
            WfdService,
            self.robot_name_ + '/wfd_service',
            callback_group=self.para_group)
        self._action_client = ActionClient(
            self, GroupCoordinator,
            self.robot_name_ + '/group_coordinator_action')

        self.wfd_action_client = ActionClient(self, WfdAction,
                                              self.robot_name_ + '/wfd_action')

        self.local_map_callback_lock_ = False
        map_topic = ''
        if self.robot_name_ == '':
            map_topic = '/map'
        else:
            map_topic = '/' + self.robot_name_ + '/map'
        self.local_map_sub_ = self.create_subscription(OccupancyGrid,
                                                       map_topic,
                                                       self.localMapCallback,
                                                       10)

        # self.discover_beacon_pub_ = self.create_publisher(String, 'robot_beacon', 10)
        self.debug_merge_frontiers_pub_ = self.create_publisher(
            OccupancyGrid, self.robot_name_ + '/merged_frontiers_debug', 10)
        self.debug_merge_map_pub_ = self.create_publisher(
            OccupancyGrid, self.robot_name_ + '/merged_map_debug', 10)
        self.debug_frontier_pub_ = self.create_publisher(
            OccupancyGrid, self.robot_name_ + '/frontier_map_debug', 10)
        self.inflated_map_pub_ = self.create_publisher(
            OccupancyGrid, self.robot_name_ + '/inflated_map_debug', 10)

        self.init_offset_dict_ = dict()
        self.init_offset_to_current_robot_dict_ = dict()

        self.declare_parameters(namespace='',
                                parameters=[('robot_name', None),
                                            ('peer_list', None),
                                            ('simulation_mode', None),
                                            ('tb0_init_offset', None),
                                            ('tb1_init_offset', None),
                                            ('tb2_init_offset', None),
                                            ('tb3_init_offset', None),
                                            ('tb4_init_offset', None),
                                            ('tb5_init_offset', None),
                                            ('tb6_init_offset', None),
                                            ('tb7_init_offset', None),
                                            ('tb8_init_offset', None),
                                            ('tb9_init_offset', None),
                                            ('pid', None)])

        # qos_profile=qos_profile_sensor_data)
        # rclpy.spin_once(self)
        peer_list_param_name = 'peer_list'

        param_robot_peers_ = self.get_parameter(peer_list_param_name).value
        print('robot peers from param file:')
        i = 0
        while i < self.total_robot_num_:
            self.persistent_robot_peers_.append(param_robot_peers_[i])
            i = i + 1

        print(self.persistent_robot_peers_)
        print(len(self.persistent_robot_peers_))

        self.e_util = ExploreUtil()

        self.r_interface_ = RobotControlInterface(self.robot_name_)
        self.r_interface_.debugPrint()

        for peer in self.persistent_robot_peers_:
            if peer != self.robot_name_:
                self.get_map_value_client_map_[peer] = self.create_client(
                    GetPeerMapValueOnCoords,
                    peer + '/get_map_value_on_coords',
                    callback_group=self.para_group)
                self.get_local_frontier_target_pt_client_map_[
                    peer] = self.create_client(
                        GetLocalFrontierTargetPt,
                        peer + '/get_local_frontier_target_pt_service',
                        callback_group=self.para_group)
        # self.peer_interface_ = PeerInterfaceNode(self.robot_name_)
        self.tic_ = 0

        # self.group_coordinator_ = GroupCoordinator(self.robot_name_)

        #support function update()
        self.window_frontiers = None
        self.window_frontiers_msg = None
        self.window_frontiers_rank = None

        self.peer_state_pid_list_ = None

        self.going_to_target_failed_times_ = 0
        self.is_leader_in_current_cluster = False
        self.last_failed_frontier_pt_ = Pose()
        self.is_action_finished_ = True
        self.is_wfd_action_finished_ = True
        self.group_action_result_pose_ = None
        self.group_action_result_return_state_ = None
        self.group_action_result_check_pt_list_ = None
        self.group_action_result_dist_to_f_list_ = None
        self.group_action_result_f_list_ = None
        self.peer_track_list_ = None

    def setRobotState(self, state):
        self.current_state_ = state
        # self.peer_interface_.current_state_ = state

    def robotPoseCallback(self, msg):
        # self.current_pos_[1] = msg.y
        self.current_pose_local_frame_.pose.position.x = msg.x
        self.current_pose_local_frame_.pose.position.y = msg.y
        self.current_pose_local_frame_.pose.position.z = 0.0
        self.current_pose_local_frame_.pose.orientation.x = 0.0
        self.current_pose_local_frame_.pose.orientation.y = 0.0
        self.current_pose_local_frame_.pose.orientation.z = 0.0
        self.current_pose_local_frame_.pose.orientation.w = 1.0
        self.current_pos_ = (msg.x, msg.y)

    def getLocalMapAndFrontierCompressCallback(self, request, response):
        # self.get_logger().warn('{} getLocalMapAndFrontierCompressRequest'.format(self.robot_name_))
        if self.inflated_local_map_ == None:
            self.inflated_local_map_ = OccupancyGrid()
        local_map_bytes = pickle.dumps(self.inflated_local_map_)
        # self.get_logger().warn('before compressed map size:{}'.format(len(local_map_bytes)))
        compressed_local_map_bytes = lzma.compress(local_map_bytes)
        # self.get_logger().warn('after compressed map size:{}'.format(len(compressed_local_map_bytes)))
        # print(compressed_local_map_bytes)

        # response.map_compress = []
        # for i in range(len(compressed_local_map_bytes)):
        #     response.map_compress.append(compressed_local_map_bytes[i])
        response.map_compress = list(compressed_local_map_bytes)
        # response.map_compress = [b'a',b'c',b'r',b'w']
        response.local_frontier = self.local_frontiers_msg_
        # self.get_logger().warn('send map time: {}'.format(time.time()))
        return response

    def getLocalMapAndFrontierCallback(self, request, response):

        self.get_logger().warn('{} getLocalMapAndFrontierRequest'.format(
            self.robot_name_))
        if self.inflated_local_map_ == None:
            self.inflated_local_map_ = OccupancyGrid()
        response.map = self.inflated_local_map_
        response.local_frontier = self.local_frontiers_msg_
        self.get_logger().warn('send map time: {}'.format(time.time()))

        return response

    def getRobotCurrentPos(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.world_frame_,
                self.local_fixed_frame_,
                when,
                timeout=Duration(seconds=5.0))
            # self.get_logger().info('Got {}'.format(repr(transform)))
            self.current_pos_ = (transform.transform.translation.x,
                                 transform.transform.translation.y)
            self.current_pose_local_frame_.pose.position.x = self.current_pos_[
                0]
            self.current_pose_local_frame_.pose.position.y = self.current_pos_[
                1]
            self.current_pose_local_frame_.pose.position.z = 0.0
            self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
            self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
            self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
            self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w

            # t_1 = time.time()
            # self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))

            return True
        except LookupException as e:
            self.get_logger().error('failed to get transform {}'.format(
                repr(e)))
            return False

    def localMapCallback(self, map_msg):
        #do a window_WFD from robot's current position, get a set of new frontiers, and integrate the new found frontiers with existing self.local_frontiers_
        #self.get_logger().warn('{}:localMapCallback'.format(self.tic_))
        mutex = Lock()
        mutex.acquire()
        if self.local_map_callback_lock_ == True:
            return
        self.local_map_callback_lock_ = True
        self.local_map_ = map_msg

        #self.get_logger().warn('{}:before inflateMap'.format(self.tic_))
        # self.inflated_local_map_ = self.e_util.inflateMap(self.local_map_, 5)
        self.inflated_local_map_ = self.local_map_

        #self.get_logger().warn('{}:after inflateMap'.format(self.tic_))
        self.inflated_map_pub_.publish(self.local_map_)
        self.local_map_callback_lock_ = False
        self.tic_ = self.tic_ + 1
        mutex.release()

    def generateFrontierDebugMap(self, frontier_msg_list, current_map):
        local_map_dw = current_map.info.width
        local_map_dh = current_map.info.height
        frontiers_index_list_w = []
        frontiers_index_list_h = []

        # for f_connect in local_frontiers_cell:
        #     for f_cell in f_connect:
        #         frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
        for f_connect_msg in frontier_msg_list:
            for f_pt in f_connect_msg.frontier:
                f_cell = ((int)(
                    (f_pt.point.x - current_map.info.origin.position.x) /
                    current_map.info.resolution), (int)(
                        (f_pt.point.y - current_map.info.origin.position.y) /
                        current_map.info.resolution))
                frontiers_index_list_h.append(f_cell[1])
                frontiers_index_list_w.append(f_cell[0])

        frontier_debug_map = OccupancyGrid()
        frontier_debug_map.header = copy.deepcopy(current_map.header)
        frontier_debug_map.info = copy.deepcopy(current_map.info)

        temp_array = np.asarray(current_map.data,
                                dtype=np.int8).reshape(local_map_dh,
                                                       local_map_dw)
        temp_array[:] = (int)(-1)
        temp_array[frontiers_index_list_h, frontiers_index_list_w] = 0
        # temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
        frontier_debug_map.data = temp_array.ravel().tolist()
        # for idx in frontiers_index_list:
        #     if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
        #         continue
        #     frontier_debug_map.data[int(idx)] = 0
        # frontier_map_width = frontier_debug_map.info.width
        # frontier_map_height = frontier_debug_map.info.height
        return frontier_debug_map

    def updateMultiHierarchicalCoordination(self):
        #state of current robot: GOING_TO_TARGET, WAITING_NEW_TARGET, REACH_TARGET, FINISH_TARGET_WINDOW_NOT_DONE,
        if self.current_state_ == self.e_util.SYSTEM_INIT:
            #robot rotate inplace
            self.r_interface_.rotateNCircles(1, 0.4)
            # self.updateWindowWFD()
            self.r_interface_.stopAtPlace()
            #self.current_state_ = self.TEST_MERGE_MAP
            self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
        elif self.current_state_ == self.e_util.GOING_TO_TARGET:
            self.get_logger().error('updateMulti: enter GOING_TO_TARGET')

            if self.current_target_pos_ == None:
                self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
                return
            #self.get_logger().warn('go to target:({},{}), orientation({},{},{},{})'.format(self.current_target_pos_.position.x, self.current_target_pos_.position.y, self.current_target_pos_.orientation.x, self.current_target_pos_.orientation.y, self.current_target_pos_.orientation.z, self.current_target_pos_.orientation.w))
            self.r_interface_.navigateToPoseSwarmSimulationFunction(
                self.current_target_pos_)
            # self.getRobotCurrentPos()
            # direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
            # is_thread_started = False
            # check_environment_thread = Thread(target=self.checkEnvironmentFunction)
            # while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:

            #     # self.get_logger().error('start checking environment...')
            #     # self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
            #     # self.getRobotCurrentPos()
            #     current_direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
            #     # self.get_logger().warn("navigating to target {},{}".format(self.current_target_pos_.position.x, self.current_target_pos_.position.y ))
            #     # current_direct_length_square = 10.0
            #     if current_direct_length_square < 2.0*2.0:
            #         if is_thread_started == False:
            #             check_environment_thread.start()

            #             is_thread_started = True
            #         # return

            #     pass
            while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:
                pass
            if self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_DONE:
                self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
            elif self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_FAILED:
                self.get_logger().error('NAVIGATION_FAILED')

                current_target_cell = ((int)(
                    (self.current_target_pos_.position.x -
                     self.inflated_local_map_.info.origin.position.x) /
                    self.inflated_local_map_.info.resolution), (int)(
                        (self.current_target_pos_.position.y -
                         self.inflated_local_map_.info.origin.position.y) /
                        self.inflated_local_map_.info.resolution))
                # self.getRobotCurrentPos()
                # current_cell = ((int)((self.current_pos_[0] - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) ,  (int)((self.current_pos_[1] - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
                updated_cell = self.e_util.getFreeNeighborRandom(
                    current_target_cell, self.inflated_local_map_, 30, 50)
                if updated_cell == None:
                    self.get_logger().warn(
                        'updated_cell is None, CHECK_ENVIRONMENT now!!!!!')
                    self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
                    return
                updated_target_pt = (
                    updated_cell[0] * self.inflated_local_map_.info.resolution
                    + self.inflated_local_map_.info.origin.position.x,
                    updated_cell[1] * self.inflated_local_map_.info.resolution
                    + self.inflated_local_map_.info.origin.position.y)
                self.current_target_pos_.position.x = updated_target_pt[0]
                self.current_target_pos_.position.y = updated_target_pt[1]

                if self.previous_state_ != self.e_util.GOING_TO_TARGET:
                    self.get_logger().error(
                        'going_to_target_failed_times_ = 000000000000')
                    self.going_to_target_failed_times_ = 0
                else:
                    self.going_to_target_failed_times_ += 1
                    self.get_logger().error(
                        'going_to_target_failed_times_ = {}{}{}{}{}'.format(
                            self.going_to_target_failed_times_,
                            self.going_to_target_failed_times_,
                            self.going_to_target_failed_times_,
                            self.going_to_target_failed_times_,
                            self.going_to_target_failed_times_))

                self.previous_state_ = self.current_state_
                if self.going_to_target_failed_times_ > 10:
                    self.get_logger().warn(
                        'going_to_target_failed_times_ > 10')
                    self.get_logger().warn(
                        'going_to_target_failed_times_ > 10')
                    self.get_logger().warn(
                        'going_to_target_failed_times_ > 10')
                    self.get_logger().warn(
                        'going_to_target_failed_times_ > 10')
                    self.get_logger().warn(
                        'going_to_target_failed_times_ > 10')
                    self.last_failed_frontier_pt_ = self.current_target_pos_
                    self.going_to_target_failed_times_ = 0
                    self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
                else:
                    self.get_logger().error(
                        'retry same target again!!!!!!!!!!!!!!!!')
                    self.setRobotState(self.e_util.GOING_TO_TARGET)

            # if is_thread_started == True:
            #     check_environment_thread.join()
            #     self.current_target_pos_ = self.next_target_pos_
            #     is_thread_started == False
            #     self.setRobotState(self.e_util.GOING_TO_TARGET)

            self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
        elif self.current_state_ == self.e_util.CHECK_ENVIRONMENT:
            self.get_logger().error('Enter CHECK_ENVIRONMENT1')

            #check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
            #if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE

            wfd_response_future = self.send_wfd_service_request()
            # self.send_wfd_action_goal()
            # while self.is_wfd_action_finished_ != True:
            #     pass
            while not wfd_response_future.done():
                pass
            wfd_response = wfd_response_future.result()
            # self.get_logger().error('trying to get response from wfd service')
            # rclpy.spin_once(self)
            # wfd_response = wfd_response_future.result()
            self.get_logger().error('got response from wfd service')

            self.local_frontiers_msg_ = wfd_response.local_frontiers
            self.window_frontiers_msg = wfd_response.window_frontiers
            self.window_frontiers_rank = wfd_response.window_frontiers_rank

            # self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
            # if self.window_frontiers == -1 or self.window_frontiers == -2:
            #     self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
            #     self.current_state_ = self.e_util.CHECK_ENVIRONMENT
            # else:

            self.send_group_coordinator_goal()
            while self.is_action_finished_ != True:
                pass

            if self.group_action_result_return_state_ == 1:
                self.current_target_pos_ = self.group_action_result_pose_
            elif self.group_action_result_return_state_ == 2:
                #check window_frontier_pt, if 1), all window_f_pt covered, then go to the local_f_pt that is furthest from peers; 2) not all window_f_pt covered,
                # go to closest uncovered window_f_pt.
                all_window_f_covered_by_peers = True
                f_pt_index_to_peer_value_map = self.send_get_map_values_request(
                    self.group_action_result_check_pt_list_)

                uncovered_f_pt_index_list = []
                for f_pt_index in f_pt_index_to_peer_value_map:
                    is_covered = False
                    for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
                        if f_pt_value != -1 and f_pt_value < 80:
                            is_covered = True
                            break
                    if is_covered == False:
                        uncovered_f_pt_index_list.append(f_pt_index)

                if len(uncovered_f_pt_index_list) > 0:
                    all_window_f_covered_by_peers = False
                    #go to closest uncovered window_f_pt
                    closest_dist = 10000000
                    closest_index = 0
                    for index in range(
                            len(self.group_action_result_dist_to_f_list_)):
                        if index in uncovered_f_pt_index_list:
                            if self.group_action_result_dist_to_f_list_[
                                    index] < closest_dist:
                                closest_dist = self.group_action_result_dist_to_f_list_[
                                    index]
                                closest_index = index
                    closest_window_f_pt = Pose()
                    closest_window_f_pt.position.x = self.group_action_result_f_list_[
                        closest_index].x
                    closest_window_f_pt.position.y = self.group_action_result_f_list_[
                        closest_index].y
                    self.current_target_pos_ = closest_window_f_pt

                else:
                    all_window_f_covered_by_peers = True
                    #go to the furthest local_f_pt from peers
                    self.current_target_pos_ = self.group_action_result_pose_

            elif self.group_action_result_return_state_ == 3:
                #check local_frontier_pt, if 1), all local_f_pt covered, then merge map, 2), not all local_f_pt covered, go to closest local_f_pt
                all_local_f_covered_by_peers = True
                f_pt_index_to_peer_value_map = self.send_get_map_values_request(
                    self.group_action_result_check_pt_list_)

                uncovered_f_pt_index_list = []
                for f_pt_index in f_pt_index_to_peer_value_map:
                    is_covered = False
                    for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
                        if f_pt_value != -1 and f_pt_value < 80:
                            is_covered = True
                            break
                    if is_covered == False:
                        uncovered_f_pt_index_list.append(f_pt_index)

                if len(uncovered_f_pt_index_list) > 0:
                    all_local_f_covered_by_peers = False
                    #go to closest uncovered local_f_pt, doesn't another action request
                    closest_dist = 10000000
                    closest_index = 0
                    for index in range(
                            len(self.group_action_result_dist_to_f_list_)):
                        if index in uncovered_f_pt_index_list:
                            if self.group_action_result_dist_to_f_list_[
                                    index] < closest_dist:
                                closest_dist = self.group_action_result_dist_to_f_list_[
                                    index]
                                closest_index = index
                    closest_window_f_pt = Pose()
                    closest_window_f_pt.position.x = self.group_action_result_f_list_[
                        closest_index].x
                    closest_window_f_pt.position.y = self.group_action_result_f_list_[
                        closest_index].y
                    self.current_target_pos_ = closest_window_f_pt
                else:
                    all_local_f_covered_by_peers = True

                    #request local_frontiers target points from peers, find the one that is furthest from peer tracks
                    local_frontier_target_pt_list = self.send_get_local_frontiers_target_value_request(
                    )
                    largest_dist = -1.0
                    furthest_target_pt = Point()
                    if len(local_frontier_target_pt_list) > 0:
                        for target_pt in local_frontier_target_pt_list:
                            min_dist = 10000000000
                            for track in self.peer_track_list_:
                                dist = (target_pt.x -
                                        track.x) * (target_pt.x - track.x) + (
                                            target_pt.y -
                                            track.y) * (target_pt.y - track.y)
                                if dist < min_dist:
                                    min_dist = dist
                            if min_dist > largest_dist:
                                largest_dist = min_dist
                                furthest_target_pt = target_pt
                        target_pose = Pose()
                        target_pose.position.x = furthest_target_pt.x
                        target_pose.position.y = furthest_target_pt.y
                        self.current_target_pos_ = target_pose
                    else:
                        self.current_target_pos_ = None
                    #only case that requires another action request
                    #send another action request to merge map and find closest merged_f_pt

            # self.group_coordinator_.setPeerInfo(self.persistent_robot_peers_, peer_pose_dict, cluster_list, cluster_pose_dict, self.window_frontiers, self.window_frontiers_rank, self.local_frontiers_, self.local_frontiers_msg_, self.inflated_local_map_, self.peer_interface_.init_offset_dict_, self.last_failed_frontier_pt_)
            # self.current_target_pos_ = self.group_coordinator_.hierarchicalCoordinationAssignment()
            if self.current_target_pos_ == None:
                self.get_logger().error(
                    'finish local_frontiers, done for current robot')
                self.setRobotState(self.e_util.FINISH_TASK)
            else:
                self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
                self.setRobotState(self.e_util.GOING_TO_TARGET)

                # no window frontiers available, find target frontier from self.local_frontiers_msg_, considering distance from peer robot_tracks
        elif self.current_state_ == self.e_util.FINISH_TASK:
            self.r_interface_.rotateNCircles(1, 0.4)
            self.get_logger().error(
                'finish local_frontiers, done for current robot')

    def send_group_coordinator_goal(self):
        self.get_logger().error('Enter send_group_coordinator_goal')

        self._action_client.wait_for_server()
        while self.is_action_finished_ == False:
            pass
        self.get_logger().error('send_group_coordinator_goal ready')

        self.is_action_finished_ = False
        goal_msg = GroupCoordinator.Goal()
        for peer in self.persistent_robot_peers_:
            peer_msg = String()
            if peer != self.robot_name_:
                peer_msg.data = peer
                goal_msg.peer_list.append(peer_msg)
        # if self.getRobotCurrentPos():
        # else:
        #     self.get_logger().error('(send_group_coordinator_goal) fail to get robot current pose')
        #     return

        goal_msg.robot_pose_local_frame = self.current_pose_local_frame_
        goal_msg.window_frontiers = self.window_frontiers_msg
        goal_msg.window_frontiers_rank = self.window_frontiers_rank
        goal_msg.local_frontiers = self.local_frontiers_msg_
        goal_msg.local_inflated_map = self.inflated_local_map_
        goal_msg.last_failed_frontier_pt.pose = self.last_failed_frontier_pt_
        # goal_uuid = 0
        # if self.robot_name_ == 'tb0':
        #     goal_uuid = 0
        # elif self.robot_name_ == 'tb1':
        #     goal_uuid = 1
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_group_coordinator_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def feedback_group_coordinator_callback(self, feedback):
        pass

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('group coordinator goal rejected')
            return

        self.get_logger().warn('group coordinator goal accepted')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
            self.group_action_result_pose_ = result.current_target_pose
            self.group_action_result_check_pt_list_ = result.check_pt_list
            self.group_action_result_return_state_ = result.return_state
            self.group_action_result_dist_to_f_list_ = result.dist_to_f_list
            self.group_action_result_f_list_ = result.f_list
            self.peer_track_list_ = result.peer_track_list
            self.is_action_finished_ = True
        else:
            self.get_logger().error(
                'Goal failed with status: {}'.format(status))

    def send_wfd_service_request(self):
        while not self.wfd_service_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('wfd servie not available,wait')
        req = WfdService.Request()
        # if self.getRobotCurrentPos():
        # self.get_logger().error('Enter send_wfd_service_request')

        req.robot_pose_local_frame = self.current_pose_local_frame_.pose
        req.local_frontiers = self.local_frontiers_msg_
        req.local_inflated_map = self.inflated_local_map_
        return self.wfd_service_client.call_async(req)

    def send_get_map_values_request(self, check_pt_list):
        f_pt_index_to_peer_value_map = dict()
        self.get_logger().warn('send_get_map_values_request')
        for peer in self.persistent_robot_peers_:
            if peer != self.robot_name_:
                while not self.get_map_value_client_map_[
                        peer].wait_for_service(timeout_sec=1.0):
                    pass
                req = GetPeerMapValueOnCoords.Request()
                req.query_pt_list = check_pt_list
                print("request " + peer)
                self.get_logger().info('before')
                res_future = self.get_map_value_client_map_[peer].call_async(
                    req)
                while not res_future.done():
                    pass
                res = res_future.result()
                self.get_logger().info('after')
                value_list_result = res.pt_value_list
                for v_index in range(len(check_pt_list)):
                    if v_index in f_pt_index_to_peer_value_map:
                        f_pt_index_to_peer_value_map[v_index].append(
                            value_list_result[v_index])
                    else:
                        f_pt_index_to_peer_value_map[v_index] = []
                        f_pt_index_to_peer_value_map[v_index].append(
                            value_list_result[v_index])

        return f_pt_index_to_peer_value_map

    def send_get_local_frontiers_target_value_request(self):
        local_frontier_target_pt_list = []
        for peer in self.persistent_robot_peers_:
            if peer != self.robot_name_:
                while not self.get_local_frontier_target_pt_client_map_[
                        peer].wait_for_service(timeout_sec=1.0):
                    pass
                req = GetLocalFrontierTargetPt.Request()
                self.get_logger().info('before')
                res_future = self.get_local_frontier_target_pt_client_map_[
                    peer].call_async(req)
                self.get_logger().info('after')
                while not res_future.done():
                    pass
                response = res_future.result()
                target_pt_list = response.local_frontier_target_pt
                local_frontier_target_pt_list.extend(target_pt_list)

        return local_frontier_target_pt_list

    def send_wfd_action_goal(self):
        self.wfd_action_client.wait_for_server()
        while self.is_wfd_action_finished_ == False:
            pass
        self.is_wfd_action_finished_ = False
        goal_msg = WfdAction.Goal()
        goal_msg.local_frontiers = self.local_frontiers_msg_
        # if self.getRobotCurrentPos():
        # else:
        #     self.get_logger().error('(send_wfd_goal) fail to get robot current pose')
        #     return
        goal_msg.robot_pose_local_frame = self.current_pose_local_frame_.pose
        goal_msg.local_inflated_map = self.inflated_local_map_

        self._send_wfd_goal_future = self.wfd_action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_wfd_action_callback)

        self._send_wfd_goal_future.add_done_callback(
            self.wfd_goal_response_callback)

    def feedback_wfd_action_callback(self, feedback):
        pass

    def wfd_goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('wfd goal rejected')
            return

        self.get_logger().warn('wfd goal accepted')
        self._get_wfd_result_future = goal_handle.get_result_async()
        self._get_wfd_result_future.add_done_callback(
            self.get_wfd_result_callback)

    def get_wfd_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
            self.local_frontiers_msg_ = result.local_frontiers
            self.window_frontiers_msg = result.window_frontiers
            self.window_frontiers_rank = result.window_frontiers_rank
            # self.window_frontiers_.clear()
            # for wf_msg in self.window_frontiers_msg:

            self.is_wfd_action_finished_ = True
        else:
            self.get_logger().error(
                'Goal failed with status: {}'.format(status))

    def checkEnvironmentFunction(self):
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        self.get_logger().error('Enter CHECK_ENVIRONMENT_THREAD')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')

        #check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
        #if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
        # self.send_wfd_action_goal()
        # while self.is_wfd_action_finished_ != True:
        #     pass
        wfd_response_future = self.send_wfd_service_request()
        self.get_logger().error('got response from wfd service')
        # self.send_wfd_action_goal()
        # while self.is_wfd_action_finished_ != True:
        #     pass
        while not wfd_response_future.done():
            pass
        wfd_response = wfd_response_future.result()
        self.local_frontiers_msg_ = wfd_response.local_frontiers
        self.window_frontiers_msg = wfd_response.window_frontiers
        self.window_frontiers_rank = wfd_response.window_frontiers_rank

        # self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
        # if self.window_frontiers == -1 or self.window_frontiers == -2:
        #     self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
        #     #self.current_state_ = self.e_util.CHECK_ENVIRONMENT
        # else:
        self.send_group_coordinator_goal()
        while self.is_action_finished_ != True:
            pass
        self.get_logger().error('get result from group_coordinator_goal')

        if self.group_action_result_return_state_ == 1:
            self.get_logger().error('state 1')
            # self.current_target_pos_ = self.group_action_result_pose_
            self.next_target_pos_ = self.group_action_result_pose_
        elif self.group_action_result_return_state_ == 2:
            self.get_logger().error('state 2')

            #check window_frontier_pt, if 1), all window_f_pt covered, then go to the local_f_pt that is furthest from peers; 2) not all window_f_pt covered,
            # go to closest uncovered window_f_pt.
            all_window_f_covered_by_peers = True
            f_pt_index_to_peer_value_map = self.send_get_map_values_request(
                self.group_action_result_check_pt_list_)

            uncovered_f_pt_index_list = []
            for f_pt_index in f_pt_index_to_peer_value_map:
                is_covered = False
                for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
                    if f_pt_value != -1 and f_pt_value < 80:
                        is_covered = True
                        break
                if is_covered == False:
                    uncovered_f_pt_index_list.append(f_pt_index)

            if len(uncovered_f_pt_index_list) > 0:
                all_window_f_covered_by_peers = False
                #go to closest uncovered window_f_pt
                closest_dist = 10000000
                closest_index = 0
                for index in range(
                        len(self.group_action_result_dist_to_f_list_)):
                    if index in uncovered_f_pt_index_list:
                        if self.group_action_result_dist_to_f_list_[
                                index] < closest_dist:
                            closest_dist = self.group_action_result_dist_to_f_list_[
                                index]
                            closest_index = index
                closest_window_f_pt = Pose()
                closest_window_f_pt.position.x = self.group_action_result_f_list_[
                    closest_index].x
                closest_window_f_pt.position.y = self.group_action_result_f_list_[
                    closest_index].y
                self.next_target_pos_ = closest_window_f_pt

            else:
                all_window_f_covered_by_peers = True
                #go to the furthest local_f_pt from peers
                self.next_target_pos_ = self.group_action_result_pose_

        elif self.group_action_result_return_state_ == 3:
            self.get_logger().error('state 3')

            #check local_frontier_pt, if 1), all local_f_pt covered, then merge map, 2), not all local_f_pt covered, go to closest local_f_pt
            all_local_f_covered_by_peers = True
            f_pt_index_to_peer_value_map = self.send_get_map_values_request(
                self.group_action_result_check_pt_list_)

            uncovered_f_pt_index_list = []
            for f_pt_index in f_pt_index_to_peer_value_map:
                is_covered = False
                for f_pt_value in f_pt_index_to_peer_value_map[f_pt_index]:
                    if f_pt_value != -1 and f_pt_value < 80:
                        is_covered = True
                        break
                if is_covered == False:
                    uncovered_f_pt_index_list.append(f_pt_index)

            if len(uncovered_f_pt_index_list) > 0:
                all_local_f_covered_by_peers = False
                #go to closest uncovered local_f_pt, doesn't another action request
                closest_dist = 10000000
                closest_index = 0
                for index in range(
                        len(self.group_action_result_dist_to_f_list_)):
                    if index in uncovered_f_pt_index_list:
                        if self.group_action_result_dist_to_f_list_[
                                index] < closest_dist:
                            closest_dist = self.group_action_result_dist_to_f_list_[
                                index]
                            closest_index = index
                closest_window_f_pt = Pose()
                closest_window_f_pt.position.x = self.group_action_result_f_list_[
                    closest_index].x
                closest_window_f_pt.position.y = self.group_action_result_f_list_[
                    closest_index].y
                self.next_target_pos_ = closest_window_f_pt
            else:
                all_local_f_covered_by_peers = True

                local_frontier_target_pt_list = self.send_get_local_frontiers_target_value_request(
                )
                largest_dist = -1.0
                furthest_target_pt = Point()
                if len(local_frontier_target_pt_list) > 0:
                    for target_pt in local_frontier_target_pt_list:
                        min_dist = 10000000000
                        for track in self.peer_track_list_:
                            dist = (target_pt.x -
                                    track.x) * (target_pt.x - track.x) + (
                                        target_pt.y - track.y) * (target_pt.y -
                                                                  track.y)
                            if dist < min_dist:
                                min_dist = dist
                        if min_dist > largest_dist:
                            largest_dist = min_dist
                            furthest_target_pt = target_pt
                    target_pose = Pose()
                    target_pose.position.x = furthest_target_pt.x
                    target_pose.position.y = furthest_target_pt.y
                    self.next_target_pos_ = target_pose
                else:
                    self.next_target_pos_ = None
                # self.get_logger().error('done ,rest......')

                #only case that requires another action request
                #send another action request to merge map and find closest merged_f_pt

        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        self.get_logger().error(
            'EXIT CHECK_ENVIRONMENT_THREAD, next target:{},{}'.format(
                self.next_target_pos_.position.x,
                self.next_target_pos_.position.y))
class RobotTrackPublisherNode(Node):
    def __init__(self, robot_name, mode="real_robot"):
        super().__init__('robot_track_publisher_' + robot_name)
        self.DEBUG_ = True
        self.para_group = ReentrantCallbackGroup()
        self.local_frontiers_ = [
        ]  #list of frontier, each is list of (double, double) in the local map frame
        self.local_frontiers_msg_ = [
        ]  #list of multi_robot_interfaces.msg.Frontier
        self.global_frontiers_ = []
        self.robot_name_ = robot_name
        self.current_state_ = 0
        self.previous_state_ = -1
        #current robot_peers
        self.robot_peers_ = []
        self.mode_ = mode
        #all the robot_peers ever discovered in history, no matter current network status
        self.persistent_robot_peers_ = []
        #the ever growing merged global_map of current robot, will update with new peer_map and new local_map
        self.persistent_global_map_ = None
        #offset from robot_peers to the self.local_fixed_frame_, {'tb0': (x_offset, y_offset) , 'tb1':(x_offset, y_offset), ...}
        self.persistent_offset_from_peer_to_local_ = dict()
        #current local_map
        self.local_map_ = None
        self.inflated_local_map_ = None
        self.current_pos_ = (-1, -1)
        self.current_pose_local_frame_ = PoseStamped()
        self.current_target_pos_ = Pose()
        self.next_target_pos_ = Pose()

        self.world_frame_ = ''
        self.local_fixed_frame_ = ''
        if self.robot_name_ == '':
            self.world_frame_ = 'map'
            self.local_fixed_frame_ = 'base_link'
        else:
            self.world_frame_ = self.robot_name_ + '/map'
            self.local_fixed_frame_ = self.robot_name_ + '/base_link'

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        timer_period = 2  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.publisher_ = self.create_publisher(RobotTrack, 'robot_track', 10)
        self.robot_pose_sub_ = self.create_subscription(
            Point, self.robot_name_ + '/robot_pose', self.robotPoseCallback,
            10)
        self.robot_pose_sub_  # prevent unused variable warning

        self.init_offset_dict_ = dict()
        self.init_offset_to_current_robot_dict_ = dict()

        self.declare_parameters(namespace='',
                                parameters=[('robot_name', None),
                                            ('peer_list', None),
                                            ('simulation_mode', None),
                                            ('tb0_init_offset', None),
                                            ('tb1_init_offset', None),
                                            ('tb2_init_offset', None),
                                            ('tb3_init_offset', None),
                                            ('tb4_init_offset', None),
                                            ('tb5_init_offset', None),
                                            ('tb6_init_offset', None),
                                            ('tb7_init_offset', None),
                                            ('tb8_init_offset', None),
                                            ('tb9_init_offset', None),
                                            ('pid', None)])

        # qos_profile=qos_profile_sensor_data)
        # rclpy.spin_once(self)
        peer_list_param_name = 'peer_list'
        self.persistent_robot_peers_ = self.get_parameter(
            peer_list_param_name).value

        self.e_util = ExploreUtil()
        self.getPeerRobotInitPose()

    def getPeerRobotInitPose(self):
        for robot in self.persistent_robot_peers_:
            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]

    def robotPoseCallback(self, msg):
        self.current_pose_local_frame_.pose.position.x = msg.x
        self.current_pose_local_frame_.pose.position.y = msg.y
        self.current_pose_local_frame_.pose.position.z = 0.0
        self.current_pose_local_frame_.pose.orientation.x = 0.0
        self.current_pose_local_frame_.pose.orientation.y = 0.0
        self.current_pose_local_frame_.pose.orientation.z = 0.0
        self.current_pose_local_frame_.pose.orientation.w = 1.0

    def timer_callback(self):
        track = Point()
        if self.mode_ == "real_robot":
            self.getRobotCurrentPos()
            track.x = self.current_pos_[0] + self.init_offset_dict_[
                self.robot_name_].position.x
            track.y = self.current_pos_[1] + self.init_offset_dict_[
                self.robot_name_].position.y
        elif self.mode_ == "swarm_simulation":
            track.x = self.current_pose_local_frame_.pose.position.x
            track.y = self.current_pose_local_frame_.pose.position.y

        msg = RobotTrack()
        msg.robot_name.data = self.robot_name_
        msg.robot_track = track
        self.publisher_.publish(msg)
        # self.get_logger().info('Publishing: "%s"' % msg.robot_name.data)

    def getRobotCurrentPos(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.world_frame_,
                self.local_fixed_frame_,
                when,
                timeout=Duration(seconds=5.0))
            # self.get_logger().info('Got {}'.format(repr(transform)))
            self.current_pos_ = (transform.transform.translation.x,
                                 transform.transform.translation.y)
            self.current_pose_local_frame_.pose.position.x = self.current_pos_[
                0]
            self.current_pose_local_frame_.pose.position.y = self.current_pos_[
                1]
            self.current_pose_local_frame_.pose.position.z = 0.0
            self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
            self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
            self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
            self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w

            # t_1 = time.time()
            # self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))

            return True
        except LookupException as e:
            self.get_logger().error('failed to get transform {}'.format(
                repr(e)))
            return False
예제 #12
0
class Robot(Node):
    def __init__(self):
        super().__init__(node_name="cetautomatix")
        # Detect simulation mode
        self.simulation = True if machine() != "aarch64" else False
        self.i = 0
        self.stupid_actions = ["STUPID_1", "STUPID_2", "STUPID_3"]
        self._triggered = False
        self._position = None
        self._orientation = None
        self._start_time = None
        self._current_action = None
        self.robot = self.get_namespace().strip("/")
        # parameters interfaces
        self.side = self.declare_parameter("side", "blue")
        self.declare_parameter("length")
        self.declare_parameter("width")
        self.declare_parameter("strategy_mode")
        self.length_param = self.get_parameter("length")
        self.width_param = self.get_parameter("width")
        self.strategy_mode_param = self.get_parameter("strategy_mode")
        # Bind actuators
        self.actuators = import_module(f"actuators.{self.robot}").actuators
        # Do selftest
        self.selftest = Selftest(self)
        # Prechill engines
        self.actuators.setFansEnabled(True)
        # Stop ros service
        self._stop_ros_service = self.create_service(Trigger, "stop",
                                                     self._stop_robot_callback)
        # strategix client interfaces
        self._get_available_request = GetAvailableActions.Request()
        self._get_available_request.sender = self.robot
        self._get_available_client = self.create_client(
            GetAvailableActions, "/strategix/available")
        self._change_action_status_request = ChangeActionStatus.Request()
        self._change_action_status_request.sender = self.robot
        self._change_action_status_client = self.create_client(
            ChangeActionStatus, "/strategix/action")
        # Phararon delploy client interfaces
        self._get_trigger_deploy_pharaon_request = Trigger.Request()
        self._get_trigger_deploy_pharaon_client = self.create_client(
            Trigger, "/pharaon/deploy")
        # Slider driver
        self._set_slider_position_request = Slider.Request()
        self._set_slider_position_client = self.create_client(
            Slider, "slider_position")
        # Odometry subscriber
        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self._odom_pose_stamped = tf2_geometry_msgs.PoseStamped()
        self._odom_callback(self._odom_pose_stamped)
        self._odom_sub = self.create_subscription(Odometry, "odom",
                                                  self._odom_callback, 1)
        # Py-Trees blackboard to send NavigateToPose actions
        self.blackboard = py_trees.blackboard.Client(name="NavigateToPose")
        self.blackboard.register_key(key="goal",
                                     access=py_trees.common.Access.WRITE)
        # LCD driver direct access
        self._lcd_driver_pub = self.create_publisher(Lcd, "/obelix/lcd", 1)
        # Wait for strategix as this can block the behavior Tree
        while not self._get_available_client.wait_for_service(timeout_sec=5):
            self.get_logger().warn(
                "Failed to contact strategix services ! Has it been started ?")
        # Enable stepper drivers
        if not self.simulation:
            self._get_enable_drivers_request = SetBool.Request()
            self._get_enable_drivers_request.data = True
            self._get_enable_drivers_client = self.create_client(
                SetBool, "enable_drivers")
            self._synchronous_call(self._get_enable_drivers_client,
                                   self._get_enable_drivers_request)
        # Robot trigger service
        self._trigger_start_robot_server = self.create_service(
            Trigger, "start", self._start_robot_callback)
        if self.robot == "obelix":
            self._trigger_start_asterix_request = Trigger.Request()
            self._trigger_start_asterix_client = self.create_client(
                Trigger, "/asterix/start")

        # Reached initialized state
        self.get_logger().info("Cetautomatix ROS node has been started")

    def _synchronous_call(self, client, request):
        """Call service synchronously."""
        future = client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        try:
            response = future.result()
        except BaseException:
            response = None
        return response

    def set_slider_position(self, position: int):
        """Set slider position with position in range 0 to 255."""
        self._set_slider_position_request.position = position
        return self._synchronous_call(self._set_slider_position_client,
                                      self._set_slider_position_request)

    def fetch_available_actions(self):
        """Fetch available actions for BT."""
        response = self._synchronous_call(self._get_available_client,
                                          self._get_available_request)
        return response.available if response is not None else []

    def preempt_action(self, action):
        """Preempt an action for the BT."""
        if action is None:
            return False
        self._change_action_status_request.action = str(action)
        self._change_action_status_request.request = "PREEMPT"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = action if response.success else None
        if self._current_action is not None:
            self.get_goal_pose()
        return response.success

    def drop_current_action(self):
        """Drop an action for the BT."""
        if self._current_action is None:
            return False
        self._change_action_status_request.action = self._current_action
        self._change_action_status_request.request = "DROP"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = None
        return response.success

    def confirm_current_action(self):
        """Confirm an action for the BT."""
        if self._current_action is None:
            return False
        self._change_action_status_request.action = self._current_action
        self._change_action_status_request.request = "CONFIRM"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        self._current_action = None
        return response.success

    def start_actuator_action(self):
        self.get_logger().info(f"START ACTUATOR {self._current_action}")
        if "PHARE" in self._current_action:
            response = self._synchronous_call(
                self._get_trigger_deploy_pharaon_client,
                self._get_trigger_deploy_pharaon_request,
            )
            return response.success
        elif "GOB" in self._current_action:
            self.actuators.PUMPS[
                self.current_pump]["STATUS"] = self._current_action
            servo = self.actuators.DYNAMIXELS[self.current_pump]
            self.actuators.arbotix.setPosition(self.current_pump,
                                               servo.get("up"))
            return True
        elif "ECUEIL" in self._current_action:
            pump_list = []
            i = 0
            for pump_id, pump_dict in self.actuators.PUMPS.items():
                if pump_dict.get("type") == NO:
                    pump_list.append(pump_id)
                    self.actuators.PUMPS[pump_id]["STATUS"] = actions[
                        self._current_action].get("GOBS")[i]
                    i += 1
            self.actuators.setPumpsEnabled(True, pump_list)
            # TODO: Fermer Herse
            self.set_slider_position(255)
            return True
        elif "CHENAL" in self._current_action:
            color = "GREEN" if "VERT" in self._current_action else "RED"
            pump_list = []
            for pump_id, pump_dict in self.actuators.PUMPS.items():
                if (pump_dict.get("type") == self.drop
                        and pump_dict.get("STATUS") == color):
                    pump_list.append(pump_id)
            if self.drop == NC:
                for pump in pump_list:
                    servo = self.actuators.DYNAMIXELS[pump]
                    self.actuators.arbotix.setPosition(pump, servo.get("down"))
            else:
                self.set_slider_position(100)
                # TODO: Ouvrir herse
            self.actuators.setPumpsEnabled(False, pump_list)
            for pump in pump_list:
                del self.actuators.PUMPS[pump]["STATUS"]
            return True
        else:
            return True

    def trigger_pavillons(self):
        self.get_logger().info("Triggered pavillons")
        self.actuators.raiseTheFlag()
        self._change_action_status_request.action = "PAVILLON"
        self._change_action_status_request.request = "CONFIRM"
        response = self._synchronous_call(self._change_action_status_client,
                                          self._change_action_status_request)
        if response is None:
            return False
        return response.success

    def _start_robot_callback(self, req, resp):
        """Start robot."""
        self._triggered = True
        self.get_logger().info("Triggered robot starter")
        if self.robot == "obelix":
            # TODO : Fix sync call
            # Thread(target=self._synchronous_call, args=[self._trigger_start_asterix_client, self._trigger_start_asterix_request]).start()
            self.get_logger().info("Obelix triggered Asterix")
        self._start_time = self.get_clock().now().nanoseconds * 1e-9
        lcd_msg = Lcd()
        lcd_msg.line = 0
        lcd_msg.text = f"{self.robot.capitalize()} running".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        lcd_msg.line = 1
        lcd_msg.text = "Score: 0".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        if hasattr(resp, "success"):
            resp.success = True
        return resp

    def _stop_robot_callback(self, req, resp):
        """Stop robot / ROS."""
        self.stop_ros(shutdown=False)
        resp.sucess = True
        return resp

    def triggered(self):
        """Triggered var."""
        return self._triggered

    def _odom_callback(self, msg):
        try:
            # Get latest transform
            tf = self._tf_buffer.lookup_transform(
                "map", "base_link", Time(), timeout=Duration(seconds=1.0))
            self._odom_pose_stamped.header = msg.header
            self._odom_pose_stamped.pose = msg.pose.pose
            tf_pose = tf2_geometry_msgs.do_transform_pose(
                self._odom_pose_stamped, tf)
        except LookupException:
            self.get_logger().warn(
                "Failed to lookup_transform from map to odom")
            return
        except ConnectivityException:
            self.get_logger().warn(
                "ConnectivityException, 'map' and 'base_link' are not part of the same tree"
            )
            return
        self._position = (tf_pose.pose.position.x, tf_pose.pose.position.y)
        self._orientation = self.quaternion_to_euler(
            tf_pose.pose.orientation.x,
            tf_pose.pose.orientation.y,
            tf_pose.pose.orientation.z,
            tf_pose.pose.orientation.w,
        )

    def euler_to_quaternion(self, yaw, pitch=0, roll=0):
        """Conversion between euler angles and quaternions."""
        qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
            roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
        qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
            roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
        qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
            roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2)
        qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
            roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
        return (qx, qy, qz, qw)

    def quaternion_to_euler(self, x, y, z, w):
        """Conversion between quaternions and euler angles."""
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        X = math.atan2(t0, t1)
        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        Y = math.asin(t2)
        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        Z = math.atan2(t3, t4)
        return (X, Y, Z)

    def get_position(self, element, initial_orientation, diff_orientation,
                     offset):
        element = elements[element]
        element_x, element_y = element.get("X"), element.get("Y")
        offset_x, offset_y = offset[0], offset[1]

        alpha = initial_orientation[0] - np.pi / 2
        v = (
            offset_x * np.cos(alpha) - offset_y * np.sin(alpha),
            offset_x * np.sin(alpha) + offset_y * np.cos(alpha),
        )
        v = (
            v[0] * np.cos(diff_orientation) - v[1] * np.sin(diff_orientation),
            v[0] * np.sin(diff_orientation) + v[1] * np.cos(diff_orientation),
        )
        return (element_x - v[0], element_y - v[1])

    def get_orientation(self, element, robot_pos, initial_orientation):
        element = elements[element]
        element_x, element_y = element.get("X"), element.get("Y")
        robot_pos_x, robot_pos_y = robot_pos[0], robot_pos[1]

        u = (np.cos(initial_orientation[0]), np.sin(initial_orientation[0]))
        u = u / (np.sqrt(u[0]**2 + u[1]**2))
        if element.get("Rot") is None:
            v = (element_x - robot_pos_x, element_y - robot_pos_y)
            v = v / (np.sqrt(v[0]**2 + v[1]**2))
            theta = np.arccos(
                np.dot(u, v) /
                (np.sqrt(u[0]**2 + u[1]**2) * np.sqrt(v[0]**2 + v[1]**2)))
            if robot_pos_y <= element_y:
                theta = theta
            else:
                theta = -theta
        else:
            angle = element.get("Rot")
            theta = np.deg2rad(angle) - initial_orientation[0]
        phi = initial_orientation[0] + theta
        return (theta, phi)

    def compute_best_action(self, action_list):
        """Calculate best action to choose from its distance to the robot."""
        return self.stupid_actions[self.i % len(self.stupid_actions)]
        check = self.check_to_empty()
        self.get_logger().info(f"{check}")
        if check is not None:
            action_list = check
        if not action_list:
            return None
        coefficient_list = []
        for action in action_list:
            coefficient = 0
            element = elements[action]
            distance = np.sqrt((element["X"] - self._position[0])**2 +
                               (element["Y"] - self._position[1])**2)
            coefficient += 100 * (1 - distance / 3.6)
            coefficient += get_time_coeff(
                self.get_clock().now().nanoseconds * 1e-9 - self._start_time,
                action,
                self.strategy_mode_param.value,
            )
            coefficient_list.append(coefficient)
        best_action = action_list[coefficient_list.index(
            max(coefficient_list))]
        return best_action

    def check_to_empty(self):
        empty_behind, empty_front = True, True
        for pump_id, pump_dict in self.actuators.PUMPS.items():
            if pump_dict.get("STATUS") is None:
                if pump_dict.get("type") == NO:
                    empty_behind = False
                else:
                    empty_front = False
        self.get_logger().info(f"behind: {empty_behind}, front: {empty_front}")
        if empty_behind or empty_front:
            if self.side.value == "blue":
                return ["CHENAL_BLEU_VERT_1", "CHENAL_BLEU_ROUGE_1"]
            else:
                return ["CHENAL_JAUNE_VERT_1", "CHENAL_JAUNE_ROUGE_1"]
        return None

    def get_goal_pose(self):
        """Get goal pose for action."""
        msg = NavigateToPose_Goal()
        msg.pose.header.frame_id = "map"
        # if self._current_action is not None:
        #     offset = (0, 0)
        #     if 'GOB' in self._current_action:
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NC and not pump_dict.get('STATUS'):
        #                 self.current_pump = pump_id
        #                 offset = pump_dict.get("pos")
        #                 self.actuators.setPumpsEnabled(True, [pump_id])
        #                 break
        #     elif 'ECUEIL' in self._current_action:
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NO and pump_dict.get('STATUS') is not None:
        #                 offset = pump_dict.get("pos")
        #                 break
        #         self.set_slider_position(0)
        #     elif 'CHENAL' in self._current_action:
        #         arriere = False
        #         for pump_id, pump_dict in self.actuators.PUMPS.items():
        #             if pump_dict.get('type') == NO and pump_dict.get('STATUS') is not None:
        #                 arriere = True
        #         if arriere is True:
        #             offset = (0, -0.1)
        #             elements[self._current_action]["Rot"] = -90
        #             self.drop = NO
        #         else:
        #             offset = (0, 0.1)
        #             elements[self._current_action]["Rot"] = 90
        #             self.drop = NC
        if self._position is not None:
            offset = (0, 0)
            (theta, phi) = self.get_orientation(self._current_action,
                                                self._position,
                                                self._orientation)
            position = self.get_position(self._current_action,
                                         self._orientation, theta, offset)
            msg.pose.pose.position.x = position[0]
            msg.pose.pose.position.y = position[1]
            msg.pose.pose.position.z = 0.0
            q = self.euler_to_quaternion(phi)
            msg.pose.pose.orientation.x = q[0]
            msg.pose.pose.orientation.y = q[1]
            msg.pose.pose.orientation.z = q[2]
            msg.pose.pose.orientation.w = q[3]
        self.blackboard.goal = msg
        self.i += 1

    def stop_ros(self, shutdown=True):
        """Stop ros launch processes."""
        # Publish "Robot is done"
        lcd_msg = Lcd()
        lcd_msg.line = 0
        lcd_msg.text = f"{self.robot.capitalize()} is done!".ljust(16)
        self._lcd_driver_pub.publish(lcd_msg)
        if not self.simulation:
            # Disable stepper drivers
            self._get_enable_drivers_request.data = False
            self._synchronous_call(self._get_enable_drivers_client,
                                   self._get_enable_drivers_request)
            # Stop fans and relax servos
            self.actuators.disableDynamixels()
            self.actuators.setFansEnabled(False)
            # Shutdown ROS
            for p in psutil.process_iter(["pid", "name", "cmdline"]):
                if "ros2" in p.name() and "launch" in p.cmdline():
                    self.get_logger().warn(
                        f"Sent SIGINT to ros2 launch {p.pid}")
                    p.send_signal(SIGINT)
            # shutdown linux
            if shutdown:
                call(["shutdown", "-h", "now"])

    def destroy_node(self):
        """Handle SIGINT/global destructor."""
        self.actuators.disableDynamixels()
        self.actuators.setFansEnabled(False)
        super().destroy_node()
예제 #13
0
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
class TestBroadcasterAndListener(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()

        cls.buffer = Buffer()
        cls.node = rclpy.create_node('test_broadcaster_listener')
        cls.broadcaster = TransformBroadcaster(cls.node)
        cls.static_broadcaster = StaticTransformBroadcaster(cls.node)
        cls.listener = TransformListener(
            buffer=cls.buffer, node=cls.node, spin_thread=False)

        cls.executor = rclpy.executors.SingleThreadedExecutor()
        cls.executor.add_node(cls.node)

    @classmethod
    def tearDownClass(cls):
        rclpy.shutdown()

    def setUp(self):
        self.buffer = Buffer()
        self.listener.buffer = self.buffer

    def broadcast_transform(self, target_frame, source_frame, time_stamp):
        broadcast_transform = build_transform(
            target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)

        self.broadcaster.sendTransform(broadcast_transform)

        self.executor.spin_once()

        return broadcast_transform

    def broadcast_static_transform(self, target_frame, source_frame, time_stamp):
        broadcast_transform = build_transform(
            target_frame=target_frame, source_frame=source_frame, stamp=time_stamp)

        self.static_broadcaster.sendTransform(broadcast_transform)

        self.executor.spin_once()

        return broadcast_transform

    def test_broadcaster_and_listener(self):
        time_stamp = rclpy.time.Time(seconds=1, nanoseconds=0).to_msg()

        broadcasted_transform = self.broadcast_transform(
            target_frame='foo', source_frame='bar', time_stamp=time_stamp)

        listened_transform = self.buffer.lookup_transform(
            target_frame='foo', source_frame='bar', time=time_stamp)

        self.assertTrue(broadcasted_transform, listened_transform)

        # Execute a coroutine
        listened_transform_async = None
        coro = self.buffer.lookup_transform_async(
            target_frame='foo', source_frame='bar', time=time_stamp)
        try:
            coro.send(None)
        except StopIteration as e:
            # The coroutine finished; store the result
            coro.close()
            listened_transform_async = e.value

        self.assertTrue(broadcasted_transform, listened_transform_async)

    def test_extrapolation_exception(self):
        self.broadcast_transform(
            target_frame='foo', source_frame='bar',
            time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg())

        self.broadcast_transform(
            target_frame='foo', source_frame='bar',
            time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg())

        with self.assertRaises(ExtrapolationException) as e:
            self.buffer.lookup_transform(
                target_frame='foo', source_frame='bar',
                time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg())

        self.assertTrue(
            'Lookup would require extrapolation into the past' in str(e.exception))

        with self.assertRaises(ExtrapolationException) as e:
            self.buffer.lookup_transform(
                target_frame='foo', source_frame='bar',
                time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg())

        self.assertTrue(
            'Lookup would require extrapolation into the future' in str(e.exception))

    def test_static_broadcaster_and_listener(self):
        broadcasted_transform = self.broadcast_static_transform(
            target_frame='foo', source_frame='bar',
            time_stamp=rclpy.time.Time(seconds=1.1, nanoseconds=0).to_msg())

        listened_transform = self.buffer.lookup_transform(
            target_frame='foo', source_frame='bar',
            time=rclpy.time.Time(seconds=1.5, nanoseconds=0).to_msg())

        self.assertTrue(broadcasted_transform, listened_transform)
예제 #15
0
class VelocityChecker(Node):
    def __init__(self):
        super().__init__("velocity_checker")

        self.autoware_engage = None
        self.external_v_lim = np.nan
        self.localization_twist = Twist()
        self.localization_twist.linear.x = np.nan
        self.vehicle_twist = Twist()
        self.vehicle_twist.linear.x = np.nan
        self.self_pose = Pose()
        self.data_arr = [np.nan] * DATA_NUM
        self.count = 0

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # planning path and trajectories
        profile = rclpy.qos.QoSProfile(
            depth=1, durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)
        lane_drv = "/planning/scenario_planning/lane_driving"
        scenario = "/planning/scenario_planning"
        self.sub0 = self.create_subscription(
            PathWithLaneId,
            lane_drv + "/behavior_planning/path_with_lane_id",
            self.CallBackBehaviorPathWLid,
            1,
        )
        self.sub1 = self.create_subscription(
            Path, lane_drv + "/behavior_planning/path",
            self.CallBackBehaviorPath, 1)
        self.sub2 = self.create_subscription(
            Trajectory,
            lane_drv +
            "/motion_planning/obstacle_avoidance_planner/trajectory",
            self.CallBackAvoidTrajectory,
            1,
        )
        self.sub3 = self.create_subscription(Trajectory,
                                             lane_drv + "/trajectory",
                                             self.CallBackLaneDriveTrajectory,
                                             1)
        self.sub4 = self.create_subscription(
            Trajectory,
            scenario +
            "/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered",
            self.CallBackLataccTrajectory,
            1,
        )
        self.sub5 = self.create_subscription(Trajectory,
                                             scenario + "/trajectory",
                                             self.CallBackScenarioTrajectory,
                                             1)

        # control commands
        self.sub6 = self.create_subscription(
            ControlCommandStamped,
            "/control/trajectory_follower/control_cmd",
            self.CallBackControlCmd,
            1,
        )
        self.sub7 = self.create_subscription(VehicleCommand,
                                             "/control/vehicle_cmd",
                                             self.CallBackVehicleCmd, 1)

        # others related to velocity
        self.sub8 = self.create_subscription(Engage, "/autoware/engage",
                                             self.CallBackAwEngage, profile)
        self.sub9 = self.create_subscription(
            VelocityLimit,
            "/planning/scenario_planning/current_max_velocity",
            self.CallBackExternalVelLim,
            profile,
        )

        # self twist
        self.sub10 = self.create_subscription(TwistStamped,
                                              "/localization/twist",
                                              self.CallBackLocalizationTwist,
                                              1)
        self.sub11 = self.create_subscription(
            Odometry, "/vehicle/status/velocity_status",
            self.CallBackVehicleTwist, 1)

        # publish data
        self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped,
                                               "closest_speeds", 1)

        time.sleep(1.0)  # wait for ready to publish/subscribe

        # for publish traffic signal image
        self.create_timer(0.1, self.timerCallback)

    def printInfo(self):
        self.count = self.count % 30
        if self.count == 0:
            self.get_logger().info("")
            self.get_logger().info(
                "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered | Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd | Engage | Localization Vel | Vehicle Vel | [km/h]"
            )  # noqa: E501
        mps2kmph = 3.6
        vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph
        vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph
        vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph
        vel_obs_stop = self.data_arr[OBSTACLE_STOP] * mps2kmph
        vel_external_lim = self.external_v_lim * mps2kmph
        vel_latacc_filtered = self.data_arr[LAT_ACC] * mps2kmph
        vel_optimized = self.data_arr[VELOCITY_OPTIMIZE] * mps2kmph
        vel_ctrl_cmd = self.data_arr[CONTROL_CMD] * mps2kmph
        acc_ctrl_cmd = self.data_arr[CONTROL_CMD_ACC]
        vel_vehicle_cmd = self.data_arr[VEHICLE_CMD] * mps2kmph
        acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC]
        vel_localization = self.localization_twist.linear.x * mps2kmph
        vel_vehicle = self.vehicle_twist.linear.x * mps2kmph
        engage = ("None" if self.autoware_engage is None else
                  ("True" if self.autoware_engage is True else "False"))
        self.get_logger().info(
            "| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} | {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} | {11:>6s} | {12: 16.2f} | {13: 11.2f} |"
            .format(  # noqa: E501
                vel_map_lim,
                vel_behavior,
                vel_obs_avoid,
                vel_obs_stop,
                vel_external_lim,
                vel_latacc_filtered,
                vel_optimized,
                vel_ctrl_cmd,
                acc_ctrl_cmd,
                vel_vehicle_cmd,
                acc_vehicle_cmd,
                engage,
                vel_localization,
                vel_vehicle,
            ))
        self.count += 1

    def timerCallback(self):
        # self.get_logger().info('timer called')
        self.updatePose(REF_LINK, SELF_LINK)
        self.pub_v_arr.publish(Float32MultiArrayStamped(data=self.data_arr))
        self.printInfo()

    def CallBackAwEngage(self, msg):
        self.autoware_engage = msg.engage

    def CallBackExternalVelLim(self, msg):
        self.external_v_lim = msg.max_velocity

    def CallBackLocalizationTwist(self, msg):
        self.localization_twist = msg.twist

    def CallBackVehicleTwist(self, msg):
        self.vehicle_twist = msg.twist.twist

    def CallBackBehaviorPathWLid(self, msg):
        # self.get_logger().info('LANE_CHANGE called')
        closest = self.calcClosestPathWLid(msg)
        self.data_arr[LANE_CHANGE] = msg.points[closest].point.twist.linear.x
        return

    def CallBackBehaviorPath(self, msg):
        # self.get_logger().info('BEHAVIOR_VELOCITY called')
        closest = self.calcClosestPath(msg)
        self.data_arr[BEHAVIOR_VELOCITY] = msg.points[closest].twist.linear.x
        return

    def CallBackAvoidTrajectory(self, msg):
        # self.get_logger().info('OBSTACLE_AVOID called')
        closest = self.calcClosestTrajectory(msg)
        self.data_arr[OBSTACLE_AVOID] = msg.points[closest].twist.linear.x
        return

    def CallBackLaneDriveTrajectory(self, msg):
        # self.get_logger().info('OBSTACLE_STOP called')
        closest = self.calcClosestTrajectory(msg)
        self.data_arr[OBSTACLE_STOP] = msg.points[closest].twist.linear.x
        return

    def CallBackLataccTrajectory(self, msg):
        # self.get_logger().info('LAT_ACC called')
        closest = self.calcClosestTrajectory(msg)
        self.data_arr[LAT_ACC] = msg.points[closest].twist.linear.x
        return

    def CallBackScenarioTrajectory(self, msg):
        # self.get_logger().info('VELOCITY_OPTIMIZE called')
        closest = self.calcClosestTrajectory(msg)
        self.data_arr[VELOCITY_OPTIMIZE] = msg.points[closest].twist.linear.x
        return

    def CallBackControlCmd(self, msg):
        # self.get_logger().info('CONTROL_CMD called')
        self.data_arr[CONTROL_CMD] = msg.control.velocity
        self.data_arr[CONTROL_CMD_ACC] = msg.control.acceleration
        return

    def CallBackVehicleCmd(self, msg):
        # self.get_logger().info('VEHICLE_CMD called')
        self.data_arr[VEHICLE_CMD] = msg.control.velocity
        self.data_arr[VEHICLE_CMD_ACC] = msg.control.acceleration
        return

    def calcClosestPath(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcClosestPathWLid(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].point.pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcClosestTrajectory(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcSquaredDist2d(self, p1, p2):
        dx = p1.position.x - p2.position.x
        dy = p1.position.y - p2.position.y
        return dx * dx + dy * dy

    def updatePose(self, from_link, to_link):
        try:
            tf = self.tf_buffer.lookup_transform(from_link, to_link,
                                                 rclpy.time.Time())
            self.self_pose.position.x = tf.transform.translation.x
            self.self_pose.position.y = tf.transform.translation.y
            self.self_pose.position.z = tf.transform.translation.z
            self.self_pose.orientation.x = tf.transform.rotation.x
            self.self_pose.orientation.y = tf.transform.rotation.y
            self.self_pose.orientation.z = tf.transform.rotation.z
            self.self_pose.orientation.w = tf.transform.rotation.w
            return
        except LookupException as e:
            self.get_logger().warn(
                "No required transformation found: `{}`".format(str(e)))
            return
예제 #16
0
class InteractiveMarkerGoal(object):
    def __init__(self, root_link, tip_links):
        # tf
        self.tfBuffer = Buffer(rospy.Duration(1))
        self.tf_listener = TransformListener(self.tfBuffer)

        # giskard goal client
        # self.client = SimpleActionClient('move', ControllerListAction)
        self.client = SimpleActionClient('qp_controller/command',
                                         ControllerListAction)
        self.client.wait_for_server()

        # marker server
        self.server = InteractiveMarkerServer("eef_control")
        self.menu_handler = MenuHandler()

        for tip_link in tip_links:
            int_marker = self.make6DofMarker(
                InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
            self.server.insert(
                int_marker,
                self.process_feedback(self.server, self.client, root_link,
                                      tip_link))
            self.menu_handler.apply(self.server, int_marker.name)

        self.server.applyChanges()

    def transformPose(self, target_frame, pose, time=None):
        transform = self.tfBuffer.lookup_transform(
            target_frame, pose.header.frame_id,
            pose.header.stamp if time is not None else rospy.Time(0),
            rospy.Duration(1.0))
        new_pose = do_transform_pose(pose, transform)
        return new_pose

    def makeBox(self, msg):
        marker = Marker()

        marker.type = Marker.SPHERE
        marker.scale.x = msg.scale * 0.2
        marker.scale.y = msg.scale * 0.2
        marker.scale.z = msg.scale * 0.2
        marker.color.r = 0.5
        marker.color.g = 0.5
        marker.color.b = 0.5
        marker.color.a = 0.5

        return marker

    def makeBoxControl(self, msg):
        control = InteractiveMarkerControl()
        control.always_visible = True
        control.markers.append(self.makeBox(msg))
        msg.controls.append(control)
        return control

    def make6DofMarker(self, interaction_mode, root_link, tip_link):
        def normed_q(x, y, z, w):
            return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])

        int_marker = InteractiveMarker()

        p = PoseStamped()
        p.header.frame_id = tip_link
        p.pose.orientation.w = 1

        int_marker.header.frame_id = tip_link
        int_marker.pose.orientation.w = 1
        int_marker.pose = self.transformPose(root_link, p).pose
        int_marker.header.frame_id = root_link
        int_marker.scale = .2

        int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)

        # insert a box
        self.makeBoxControl(int_marker)
        int_marker.controls[0].interaction_mode = interaction_mode

        if interaction_mode != InteractiveMarkerControl.NONE:
            control_modes_dict = {
                InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
                InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
                InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
            }
            int_marker.name += "_" + control_modes_dict[interaction_mode]

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = "rotate_x"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(0, 0, 0, 1)
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        return int_marker

    class process_feedback(object):
        def __init__(self, i_server, client, root_link, tip_link):
            self.i_server = i_server
            self.client = client
            self.tip_link = tip_link
            self.root_link = root_link

        def __call__(self, feedback):
            if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
                print('interactive goal update')
                goal = ControllerListGoal()
                goal.type = ControllerListGoal.STANDARD_CONTROLLER
                # asd = 'asd'
                # translation
                controller = Controller()
                controller.type = Controller.TRANSLATION_3D
                controller.tip_link = self.tip_link
                controller.root_link = self.root_link

                controller.goal_pose.header = feedback.header
                controller.goal_pose.pose = feedback.pose

                controller.p_gain = 3
                controller.enable_error_threshold = True
                controller.threshold_value = 0.05
                controller.weight = 1.0
                goal.controllers.append(controller)

                # rotation
                controller = Controller()
                controller.type = Controller.ROTATION_3D
                controller.tip_link = self.tip_link
                controller.root_link = self.root_link

                controller.goal_pose.header = feedback.header
                controller.goal_pose.pose = feedback.pose

                controller.p_gain = 3
                controller.enable_error_threshold = True
                controller.threshold_value = 0.2
                controller.weight = 1.0
                goal.controllers.append(controller)

                self.client.send_goal(goal)
            self.i_server.applyChanges()
예제 #17
0
class Executor(Node):
    def __init__(self):
        super().__init__('executor')

        """Update frequency
        Single targets: {baudrate} / ({uart frame length} * {cmd length} * {number of joint})
        = 115200 / (9 * 4 * 18) ~= 177Hz <- autodetect-baudrate on maestro
        = 200000 / (9 * 4 * 1x8) ~= 300Hz <- fixed-baudrate on maestro
        Multiple targets: {baudrate} / ({uart frame length} * ({header length} + {mini-cmd length} * {number of joints}))
        = 115200 / (9 * (3 + 2 * 18)) ~= 320Hz 
        = 200000 / (9 * (3 + 2 * 18)) ~= 560Hz"""

        self.subscription = self.create_subscription(
            Path,
            'sparse_path',
            self.new_trajectory,
            10
        )
        self.spi_bridge = self.create_publisher(
            UInt8MultiArray, 'stm32_cmd', 10
        )

        self.steps_trajectory = []
        self.init_test_path()

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)
        self.broadcaster = self.create_publisher(TFMessage, '/tf', 10)
        self.publish_transform = get_transform_publisher(self.broadcaster, self.get_clock())

        self.pub = self.create_publisher(JointState, 'joint_states', 10)
        self.base_link_start_height = tf.translation_matrix((0.0, 0.0, 0.1))

        self.path_proxy = PathProxy(self.steps_trajectory, self.get_logger())
        self.trajectory_generator = TrajectoryGenerator(self.path_proxy)
        self.trajectory_encoder = TrajectoryEncoder()
        self.front_point = Pose()
        self.rear_point = Pose()
        self.init_points()

        self.inv_kin_calc = TripodInverseKinematics(self._tf_buffer, self.get_clock())

        self.prev_time = self.get_clock().now()

        first_step = TrajectoryPoint()
        first_step.timestamp = self.prev_time.nanoseconds / 10**9
        first_step.left_moving = True
        first_step.left_pose = tf.translation_matrix((0, 0, 0))
        first_step.right_pose = tf.translation_matrix((0, 0, 0))
        self.prev_trajectory = self.trajectory_generator.generate_trajectory(
            first_step)

        self.first_time = False
        self.timer_callback()
        self.first_time = False
        self.start_timer = self.create_timer(START_DELAY, self.timer_callback)
        self.timer = None
        self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback)

    def new_trajectory(self, msg: Path):
        new_path = [
            pose_to_matrix(pose_stamped.pose)
            for pose_stamped in msg.poses
        ]
        self.steps_trajectory.clear()
        self.steps_trajectory.extend(new_path)

    def timer_callback(self):
        if self.first_time:
            self.first_time = False
            self.start_timer.cancel()
            self.timer = self.create_timer(UPDATE_PERIOD, self.timer_callback)

        start = self.get_clock().now()
        diff = start.nanoseconds - self.prev_time.nanoseconds
        point_i = int(SERVO_FREQ * diff / (10 ** 9))
        print(diff/10**9, point_i)
        if point_i >= len(self.prev_trajectory):
            point_i = len(self.prev_trajectory) - 1
        print(point_i)
        current_point = self.prev_trajectory[point_i]

        new_trajectory = self.trajectory_generator.generate_trajectory(current_point)

        inversed_trajectory = [
            self.inv_kin_calc.calc_point(point)
            for point in new_trajectory
        ]
        data = self.trajectory_encoder.encode_trajectory(inversed_trajectory)[1:]

        computation_time = self.get_clock().now() - start
        cmd_size = 3 + 18 * 2
        point_k = int(SERVO_FREQ * computation_time.nanoseconds / (10 ** 9))
        points_passed = min(point_i + point_k, len(self.prev_trajectory) - 1) - point_i
        print(points_passed)

        data = bytearray([42]) + data[points_passed * cmd_size:(points_passed + 250 * 2) * cmd_size]
        spi_msg = UInt8MultiArray(data=data)
        self.spi_bridge.publish(spi_msg)

        self.prev_time = start
        self.prev_trajectory = new_trajectory[points_passed:points_passed + 250 * 2]

        front_point = self.path_proxy.first_unused()
        triangle = self.inv_kin_calc.triangle(front_point, left_side=False)
        self.publish_transform('odom', 'front_point', front_point)
        self.publish_transform('odom', 'tri_a', triangle[0])
        self.publish_transform('odom', 'tri_b', triangle[1])
        self.publish_transform('odom', 'tri_c', triangle[2])

        elapsed = self.get_clock().now() - start
        # self.get_logger().debug(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
        print(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')

    def init_points(self):
        if self._tf_buffer.can_transform('odom', 'front_point', Time()):
            transform = self._tf_buffer.lookup_transform(
                'odom', 'front_point', Time())
            self.front_point.position.x = transform.transform.translation.x
            self.front_point.position.y = transform.transform.translation.y
            self.front_point.position.z = transform.transform.translation.z
        else:
            self.publish_transform(
                'odom', 'front_point', tf.translation_matrix((0, 0, 0)))

        if self._tf_buffer.can_transform('odom', 'rear_point', Time()):
            transform = self._tf_buffer.lookup_transform(
                'odom', 'rear_point', Time())
            self.rear_point.position.x = transform.transform.translation.x
            self.rear_point.position.y = transform.transform.translation.y
            self.rear_point.position.z = transform.transform.translation.z
        else:
            self.publish_transform(
                'odom', 'rear_point', tf.translation_matrix((0, 0, 0)))

        while self.pub.get_subscription_count() < 1:
            pass
        sleep(2)
        self.publish_transform('odom', 'base_link', self.base_link_start_height)

    def init_test_path(self):
        self.steps_trajectory = [
            tf.translation_matrix((0, 0, 0)),
        ]

    def tf_viz_callback(self):
        now = self.get_clock().now()
        point_i = int(SERVO_FREQ * (now.nanoseconds - self.prev_time.nanoseconds) / (10**9))  # TODO
        if point_i >= len(self.prev_trajectory):
            point_i = len(self.prev_trajectory) - 1
        point = self.prev_trajectory[point_i]
        angles = self.inv_kin_calc.calc_point(point)

        names, positions = [], []
        for name, position in angles.items():
            names.append(name)
            positions.append(position)

        msg = JointState()
        msg.header.stamp = header_stamp(self.get_clock().now())
        msg.name = names
        msg.position = positions
        self.pub.publish(msg)

        # base = tf.concatenate_matrices(point.base_link_pose, self.base_link_height)
        base = point.base_link_pose
        self.publish_transform('odom', 'base_link', base)
        self.publish_transform('odom', 'right_tri', point.right_pose)
        self.publish_transform('odom', 'left_tri', point.left_pose)
class TrajectoryVisualizer(Node):
    def __init__(self):

        super().__init__("trajectory_visualizer")

        self.fig = plt.figure()

        self.max_vel = 0.0
        self.min_vel = 0.0
        self.min_accel = 0.0
        self.max_accel = 0.0
        self.min_jerk = 0.0
        self.max_jerk = 0.0

        # update flag
        self.update_ex_vel_lim = False
        self.update_lat_acc_fil = False
        self.update_traj_raw = False
        self.update_traj_resample = False
        self.update_traj_final = False
        self.update_lanechange_path = False
        self.update_behavior_path = False
        self.update_traj_ob_avoid = False
        self.update_traj_ob_stop = False

        self.tf_buffer = Buffer(node=self)
        self.tf_listener = TransformListener(self.tf_buffer,
                                             self,
                                             spin_thread=True)

        self.self_pose = Pose()
        self.self_pose_received = False
        self.localization_vx = 0.0
        self.vehicle_vx = 0.0

        self.trajectory_external_velocity_limited = Trajectory()
        self.trajectory_lateral_acc_filtered = Trajectory()
        self.trajectory_raw = Trajectory()
        self.trajectory_time_resampled = Trajectory()
        self.trajectory_final = Trajectory()

        self.lane_change_path = PathWithLaneId()
        self.behavior_path = Path()
        self.obstacle_avoid_traj = Trajectory()
        self.obstacle_stop_traj = Trajectory()

        self.plotted = [False] * 9
        self.sub_localization_twist = self.create_subscription(
            Odometry, "/localization/kinematic_state",
            self.CallbackLocalizationTwist, 1)
        self.sub_vehicle_twist = self.create_subscription(
            VelocityReport, "/vehicle/status/velocity_status",
            self.CallbackVehicleTwist, 1)

        # BUFFER_SIZE = 65536*100
        optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/"
        self.sub1 = message_filters.Subscriber(
            self, Trajectory,
            optimizer_debug + "trajectory_external_velocity_limited")
        self.sub2 = message_filters.Subscriber(
            self, Trajectory,
            optimizer_debug + "trajectory_lateral_acc_filtered")
        self.sub3 = message_filters.Subscriber(
            self, Trajectory, optimizer_debug + "trajectory_raw")
        self.sub4 = message_filters.Subscriber(
            self, Trajectory, optimizer_debug + "trajectory_time_resampled")
        self.sub5 = message_filters.Subscriber(
            self, Trajectory, "/planning/scenario_planning/trajectory")

        lane_driving = "/planning/scenario_planning/lane_driving"
        self.sub6 = message_filters.Subscriber(
            self, PathWithLaneId,
            lane_driving + "/behavior_planning/path_with_lane_id")
        self.sub7 = message_filters.Subscriber(
            self, Path, lane_driving + "/behavior_planning/path")
        self.sub8 = message_filters.Subscriber(
            self,
            Trajectory,
            lane_driving +
            "/motion_planning/obstacle_avoidance_planner/trajectory",
        )
        self.sub9 = message_filters.Subscriber(
            self, Trajectory, "/planning/scenario_planning/trajectory")

        self.ts1 = message_filters.ApproximateTimeSynchronizer(
            [self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5)
        self.ts1.registerCallback(self.CallbackMotionVelOptTraj)

        self.ts2 = message_filters.ApproximateTimeSynchronizer(
            [self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0)
        self.ts2.registerCallback(self.CallBackLaneDrivingTraj)

        # main process
        if PLOT_TYPE == "VEL_ACC_JERK":
            self.ani = animation.FuncAnimation(self.fig,
                                               self.plotTrajectory,
                                               interval=100,
                                               blit=True)
            self.setPlotTrajectory()
        else:
            self.ani = animation.FuncAnimation(self.fig,
                                               self.plotTrajectoryVelocity,
                                               interval=100,
                                               blit=True)
            self.setPlotTrajectoryVelocity()

        plt.show()

        return

    def test(self):
        self.updatePose("map", "base_link")

    def CallbackLocalizationTwist(self, cmd):
        self.localization_vx = cmd.twist.twist.linear.x

    def CallbackVehicleTwist(self, cmd):
        self.vehicle_vx = cmd.longitudinal_velocity

    def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5):
        print("CallbackMotionVelOptTraj called")
        self.CallBackTrajExVelLim(cmd1)
        self.CallBackTrajLatAccFiltered(cmd2)
        self.CallBackTrajRaw(cmd3)
        self.CallBackTrajTimeResampled(cmd4)
        self.CallBackTrajFinal(cmd5)

    def CallBackTrajExVelLim(self, cmd):
        self.trajectory_external_velocity_limited = cmd
        self.update_ex_vel_lim = True

    def CallBackTrajLatAccFiltered(self, cmd):
        self.trajectory_lateral_acc_filtered = cmd
        self.update_lat_acc_fil = True

    def CallBackTrajRaw(self, cmd):
        self.trajectory_raw = cmd
        self.update_traj_raw = True

    def CallBackTrajTimeResampled(self, cmd):
        self.trajectory_time_resampled = cmd
        self.update_traj_resample = True

    def CallBackTrajFinal(self, cmd):
        self.trajectory_final = cmd
        self.update_traj_final = True

    def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9):
        print("CallBackLaneDrivingTraj called")
        self.CallBackLaneChangePath(cmd6)
        self.CallBackBehaviorPath(cmd7)
        self.CallbackObstacleAvoidTraj(cmd8)
        self.CallbackObstacleStopTraj(cmd9)

    def CallBackLaneChangePath(self, cmd):
        self.lane_change_path = cmd
        self.update_lanechange_path = True

    def CallBackBehaviorPath(self, cmd):
        self.behavior_path = cmd
        self.update_behavior_path = True

    def CallbackObstacleAvoidTraj(self, cmd):
        self.obstacle_avoid_traj = cmd
        self.update_traj_ob_avoid = True

    def CallbackObstacleStopTraj(self, cmd):
        self.obstacle_stop_traj = cmd
        self.update_traj_ob_stop = True

    def setPlotTrajectoryVelocity(self):
        self.ax1 = plt.subplot(1, 1, 1)  # row, col, index(<raw*col)
        (self.im1, ) = self.ax1.plot([], [],
                                     label="0: lane_change_path",
                                     marker="")
        (self.im2, ) = self.ax1.plot([], [],
                                     label="1: behavior_path",
                                     marker="",
                                     ls="--")
        (self.im3, ) = self.ax1.plot([], [],
                                     label="2: obstacle_avoid_traj",
                                     marker="",
                                     ls="-.")
        (self.im4, ) = self.ax1.plot([], [],
                                     label="3: obstacle_stop_traj",
                                     marker="",
                                     ls="--")
        (self.im5, ) = self.ax1.plot([], [],
                                     label="4-1: opt input",
                                     marker="",
                                     ls="--")
        (self.im6, ) = self.ax1.plot(
            [], [],
            label="4-2: opt external_velocity_limited",
            marker="",
            ls="--")
        (self.im7, ) = self.ax1.plot([], [],
                                     label="4-3: opt lat_acc_filtered",
                                     marker="",
                                     ls="--")
        (self.im8, ) = self.ax1.plot([], [],
                                     label="4-4: opt time_resampled",
                                     marker="",
                                     ls="--")
        (self.im9, ) = self.ax1.plot([], [],
                                     label="4-5: opt final",
                                     marker="",
                                     ls="-")
        (self.im10, ) = self.ax1.plot([], [],
                                      label="localization twist vx",
                                      color="r",
                                      marker="*",
                                      ls=":",
                                      markersize=10)
        (self.im11, ) = self.ax1.plot([], [],
                                      label="vehicle twist vx",
                                      color="k",
                                      marker="+",
                                      ls=":",
                                      markersize=10)
        self.ax1.set_title("trajectory's velocity")
        self.ax1.legend()
        self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
        self.ax1.set_ylabel("vel [m/s]")

        return (
            self.im1,
            self.im2,
            self.im3,
            self.im4,
            self.im5,
            self.im6,
            self.im7,
            self.im8,
            self.im9,
            self.im10,
            self.im11,
        )

    def plotTrajectoryVelocity(self, data):
        self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
        if self.self_pose_received is False:
            print("plot start but self pose is not received")
            return (
                self.im1,
                self.im2,
                self.im3,
                self.im4,
                self.im5,
                self.im6,
                self.im7,
                self.im8,
                self.im9,
                self.im10,
                self.im11,
            )
        print("plot start")

        # copy
        lane_change_path = self.lane_change_path
        behavior_path = self.behavior_path
        obstacle_avoid_traj = self.obstacle_avoid_traj
        obstacle_stop_traj = self.obstacle_stop_traj
        trajectory_raw = self.trajectory_raw
        trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
        trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
        trajectory_time_resampled = self.trajectory_time_resampled
        trajectory_final = self.trajectory_final

        if self.update_lanechange_path:
            x = self.CalcArcLengthPathWLid(lane_change_path)
            y = self.ToVelListPathWLid(lane_change_path)
            self.im1.set_data(x, y)
            self.update_lanechange_path = False
            if len(y) != 0:
                self.max_vel = max(10.0, np.max(y))
                self.min_vel = np.min(y)

        if self.update_behavior_path:
            x = self.CalcArcLengthPath(behavior_path)
            y = self.ToVelListPath(behavior_path)
            self.im2.set_data(x, y)
            self.update_behavior_path = False

        if self.update_traj_ob_avoid:
            x = self.CalcArcLength(obstacle_avoid_traj)
            y = self.ToVelList(obstacle_avoid_traj)
            self.im3.set_data(x, y)
            self.update_traj_ob_avoid = False

        if self.update_traj_ob_stop:
            x = self.CalcArcLength(obstacle_stop_traj)
            y = self.ToVelList(obstacle_stop_traj)
            self.im4.set_data(x, y)
            self.update_traj_ob_stop = False

        if self.update_traj_raw:
            x = self.CalcArcLength(trajectory_raw)
            y = self.ToVelList(trajectory_raw)
            self.im5.set_data(x, y)
            self.update_traj_raw = False

        if self.update_ex_vel_lim:
            x = self.CalcArcLength(trajectory_external_velocity_limited)
            y = self.ToVelList(trajectory_external_velocity_limited)
            self.im6.set_data(x, y)
            self.update_ex_vel_lim = False

        if self.update_lat_acc_fil:
            x = self.CalcArcLength(trajectory_lateral_acc_filtered)
            y = self.ToVelList(trajectory_lateral_acc_filtered)
            self.im7.set_data(x, y)
            self.update_lat_acc_fil = False

        if self.update_traj_resample:
            x = self.CalcArcLength(trajectory_time_resampled)
            y = self.ToVelList(trajectory_time_resampled)
            self.im8.set_data(x, y)
            self.update_traj_resample = False

        if self.update_traj_final:
            x = self.CalcArcLength(trajectory_final)
            y = self.ToVelList(trajectory_final)
            self.im9.set_data(x, y)
            self.update_traj_final = False

            closest = self.calcClosestTrajectory(trajectory_final)
            if closest >= 0:
                x_closest = x[closest]
                self.im10.set_data(x_closest, self.localization_vx)
                self.im11.set_data(x_closest, self.vehicle_vx)

        # change y-range
        self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])

        return (
            self.im1,
            self.im2,
            self.im3,
            self.im4,
            self.im5,
            self.im6,
            self.im7,
            self.im8,
            self.im9,
            self.im10,
            self.im11,
        )

    def CalcArcLength(self, traj):
        s_arr = []
        ds = 0.0
        s_sum = 0.0

        if len(traj.points) > 0:
            s_arr.append(s_sum)

        for i in range(1, len(traj.points)):
            p0 = traj.points[i - 1]
            p1 = traj.points[i]
            dx = p1.pose.position.x - p0.pose.position.x
            dy = p1.pose.position.y - p0.pose.position.y
            ds = np.sqrt(dx**2 + dy**2)
            s_sum += ds
            s_arr.append(s_sum)
        return s_arr

    def CalcArcLengthPathWLid(self, traj):
        s_arr = []
        ds = 0.0
        s_sum = 0.0

        if len(traj.points) > 0:
            s_arr.append(s_sum)

        for i in range(1, len(traj.points)):
            p0 = traj.points[i - 1].point
            p1 = traj.points[i].point
            dx = p1.pose.position.x - p0.pose.position.x
            dy = p1.pose.position.y - p0.pose.position.y
            ds = np.sqrt(dx**2 + dy**2)
            s_sum += ds
            s_arr.append(s_sum)
        return s_arr

    def CalcArcLengthPath(self, traj):
        s_arr = []
        ds = 0.0
        s_sum = 0.0

        if len(traj.points) > 0:
            s_arr.append(s_sum)

        for i in range(1, len(traj.points)):
            p0 = traj.points[i - 1]
            p1 = traj.points[i]
            dx = p1.pose.position.x - p0.pose.position.x
            dy = p1.pose.position.y - p0.pose.position.y
            ds = np.sqrt(dx**2 + dy**2)
            s_sum += ds
            s_arr.append(s_sum)
        return s_arr

    def ToVelList(self, traj):
        v_list = []
        for p in traj.points:
            v_list.append(p.longitudinal_velocity_mps)
        return v_list

    def ToVelListPathWLid(self, traj):
        v_list = []
        for p in traj.points:
            v_list.append(p.point.longitudinal_velocity_mps)
        return v_list

    def ToVelListPath(self, traj):
        v_list = []
        for p in traj.points:
            v_list.append(p.longitudinal_velocity_mps)
        return v_list

    def CalcAcceleration(self, traj):
        a_arr = []
        for i in range(1, len(traj.points) - 1):
            p0 = traj.points[i - 1]
            p1 = traj.points[i]
            v0 = p0.longitudinal_velocity_mps
            v1 = p1.longitudinal_velocity_mps
            v = 0.5 * (v1 + v0)
            dx = p1.pose.position.x - p0.pose.position.x
            dy = p1.pose.position.y - p0.pose.position.y
            ds = np.sqrt(dx**2 + dy**2)
            dt = ds / max(abs(v), 0.001)
            a = (v1 - v0) / dt
            a_arr.append(a)
        if len(traj.points) > 0:
            a_arr.append(0)
            a_arr.append(0)
        return a_arr

    def CalcJerk(self, traj):
        j_arr = []
        for i in range(1, len(traj.points) - 2):
            p0 = traj.points[i - 1]
            p1 = traj.points[i]
            p2 = traj.points[i + 1]
            v0 = p0.longitudinal_velocity_mps
            v1 = p1.longitudinal_velocity_mps
            v2 = p2.longitudinal_velocity_mps

            dx0 = p1.pose.position.x - p0.pose.position.x
            dy0 = p1.pose.position.y - p0.pose.position.y
            ds0 = np.sqrt(dx0**2 + dy0**2)

            dx1 = p2.pose.position.x - p1.pose.position.x
            dy1 = p2.pose.position.y - p1.pose.position.y
            ds1 = np.sqrt(dx1**2 + dy1**2)

            dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001)
            dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001)

            a0 = (v1 - v0) / max(dt0, 0.001)
            a1 = (v2 - v1) / max(dt1, 0.001)
            j = (a1 - a0) / max(dt1, 0.001)
            j_arr.append(j)
        if len(traj.points) > 0:
            j_arr.append(0)
            j_arr.append(0)
            j_arr.append(0)
        return j_arr

    def setPlotTrajectory(self):
        self.ax1 = plt.subplot(3, 1, 1)  # row, col, index(<raw*col)
        (self.im0, ) = self.ax1.plot([], [], label="0: raw", marker="")
        (self.im1, ) = self.ax1.plot([], [],
                                     label="3: time_resampled",
                                     marker="")
        (self.im2, ) = self.ax1.plot([], [],
                                     label="4: final velocity",
                                     marker="")
        self.ax1.set_title("trajectory's velocity")
        self.ax1.legend()
        self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
        self.ax1.set_ylabel("vel [m/s]")

        self.ax2 = plt.subplot(3, 1, 2)
        self.ax2.set_xlim([0, PLOT_MAX_ARCLENGTH])
        self.ax2.set_ylim([-1, 1])
        self.ax2.set_ylabel("acc [m/ss]")
        (self.im3, ) = self.ax2.plot([], [], label="final accel")

        self.ax3 = plt.subplot(3, 1, 3)
        self.ax3.set_xlim([0, PLOT_MAX_ARCLENGTH])
        self.ax3.set_ylim([-2, 2])
        self.ax3.set_xlabel("arclength [m]")
        self.ax3.set_ylabel("jerk [m/sss]")
        (self.im4, ) = self.ax3.plot([], [], label="final jerk")

        return self.im0, self.im1, self.im2, self.im3, self.im4

    def plotTrajectory(self, data):
        print("plot called")
        self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)

        # copy
        trajectory_raw = self.trajectory_raw
        # trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
        # trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
        trajectory_time_resampled = self.trajectory_time_resampled
        trajectory_final = self.trajectory_final

        # ax1
        if self.update_traj_raw:
            x = self.CalcArcLength(trajectory_raw)
            y = self.ToVelList(trajectory_raw)
            self.im0.set_data(x, y)
            self.update_traj_raw = False
            if len(y) != 0:
                self.max_vel = max(10.0, np.max(y))
                self.min_vel = np.min(y)
                # change y-range
                self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])

        if self.update_traj_resample:
            x = self.CalcArcLength(trajectory_time_resampled)
            y = self.ToVelList(trajectory_time_resampled)
            self.im1.set_data(x, y)
            self.update_traj_resample = False

        if self.update_traj_final:
            x = self.CalcArcLength(trajectory_final)
            y = self.ToVelList(trajectory_final)
            self.im2.set_data(x, y)
            self.update_traj_final = False

            # ax2
            y = self.CalcAcceleration(trajectory_final)
            if len(y) != 0:
                self.max_accel = max(0.0, np.max(y))
                self.min_accel = min(0.0, np.min(y))
                # change y-range
                self.ax2.set_ylim([self.min_accel - 1.0, self.max_accel + 1.0])
                if len(x) == len(y):
                    self.im3.set_data(x, y)

            # ax3
            y = self.CalcJerk(trajectory_final)
            if len(y) != 0:
                self.max_jerk = max(0.0, np.max(y))
                self.min_jerk = min(0.0, np.min(y))
                # change y-range
                # self.ax3.set_ylim([self.min_jerk - 1.0, self.max_jerk + 1.0])
                self.ax3.set_ylim([-2.0, 2.0])  # fixed range
                if len(x) == len(y):
                    self.im4.set_data(x, y)

        return self.im0, self.im1, self.im2, self.im3, self.im4

    def calcClosestPath(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcClosestPathWLid(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].point.pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcClosestTrajectory(self, path):
        closest = -1
        min_dist_squared = 1.0e10
        for i in range(0, len(path.points)):
            dist_sq = self.calcSquaredDist2d(self.self_pose,
                                             path.points[i].pose)
            if dist_sq < min_dist_squared:
                min_dist_squared = dist_sq
                closest = i
        return closest

    def calcSquaredDist2d(self, p1, p2):
        dx = p1.position.x - p2.position.x
        dy = p1.position.y - p2.position.y
        return dx * dx + dy * dy

    def updatePose(self, from_link, to_link):
        try:
            tf = self.tf_buffer.lookup_transform(from_link, to_link,
                                                 rclpy.time.Time())
            self.self_pose.position.x = tf.transform.translation.x
            self.self_pose.position.y = tf.transform.translation.y
            self.self_pose.position.z = tf.transform.translation.z
            self.self_pose.orientation.x = tf.transform.rotation.x
            self.self_pose.orientation.y = tf.transform.rotation.y
            self.self_pose.orientation.z = tf.transform.rotation.z
            self.self_pose.orientation.w = tf.transform.rotation.w
            print("updatePose succeeded")
            self.self_pose_received = True
            return
        except tf2_ros.TransformException:
            self.get_logger().warn(
                "lookup transform failed: from {} to {}".format(
                    from_link, to_link))
            return

    def closeFigure(self):
        plt.close(self.fig)
예제 #19
0
class MultiExploreNode(Node):
    
    def __init__(self, robot_name, total_robot_num):
        super().__init__('multi_explore_simple_node_' + robot_name)
        self.DEBUG_ = True
        self.total_robot_num_ = total_robot_num
        self.para_group = ReentrantCallbackGroup()
        self.local_frontiers_ = []   #list of frontier, each is list of (double, double) in the local map frame
        self.local_frontiers_msg_ = [] #list of multi_robot_interfaces.msg.Frontier
        self.global_frontiers_ = []
        self.robot_name_ = robot_name 
        self.current_state_ = 0
        self.previous_state_ = -1
        #current robot_peers 
        self.robot_peers_ = []
        #all the robot_peers ever discovered in history, no matter current network status
        self.persistent_robot_peers_ = []
        #the ever growing merged global_map of current robot, will update with new peer_map and new local_map
        self.persistent_global_map_ = None
        #offset from robot_peers to the self.local_fixed_frame_, {'tb0': (x_offset, y_offset) , 'tb1':(x_offset, y_offset), ...}
        self.persistent_offset_from_peer_to_local_ = dict()    
        #current local_map
        self.local_map_ = None
        self.inflated_local_map_ = None
        self.current_pos_ = (-1, -1)
        self.current_pose_local_frame_ = PoseStamped()
        self.current_target_pos_ = Pose()
        self.next_target_pos_ = Pose()
        
        #multi robot settings
        self.peer_map_ = dict()
        self.peer_local_frontiers_ = dict()
        self.peer_data_updated_ = dict()
        self.merge_map_frontier_timeout_ = 5
        
        self.merged_map_ = None
        self.merged_frontiers_ = []

        self.world_frame_ = ''
        self.local_fixed_frame_ = ''
        if self.robot_name_ =='':
            self.world_frame_ = 'map'
            self.local_fixed_frame_ = 'base_link'
        else:
            self.world_frame_ = self.robot_name_ + '/map'
            self.local_fixed_frame_ = self.robot_name_ + '/base_link'

        self._tf_buffer = Buffer()
        self._tf_listener = TransformListener(self._tf_buffer, self)

        
        # self.local_map_srv = self.create_service(GetLocalMap, self.robot_name_ + '/get_local_map', self.getLocalMapCallback)
        self.local_map_and_frontier_srv = self.create_service(GetLocalMapAndFrontier, self.robot_name_ + '/get_local_map_and_frontier', self.getLocalMapAndFrontierCallback)

        self.local_map_and_frontier_srv = self.create_service(GetLocalMapAndFrontierCompress, self.robot_name_ + '/get_local_map_and_frontier_compress', self.getLocalMapAndFrontierCompressCallback)

        self.get_map_value_srv = self.create_service(GetPeerMapValueOnCoords, self.robot_name_ + '/get_map_value_on_coords', self.getMapValueOnCoordsCallback)
        self.wfd_service_client = self.create_client(WfdService, self.robot_name_ + '/wfd_service')
        self._action_client = ActionClient(self, GroupCoordinator, self.robot_name_ + '/group_coordinator_action')

        self.wfd_action_client = ActionClient(self, WfdAction, self.robot_name_ + '/wfd_action')

        self.local_map_callback_lock_ = False
        map_topic = ''
        if self.robot_name_ == '':
            map_topic = '/map'
        else:
            map_topic = '/' + self.robot_name_ + '/map'
        self.local_map_sub_ = self.create_subscription(
            OccupancyGrid,
            map_topic,
            self.localMapCallback,
            10)
        # self.discover_beacon_pub_ = self.create_publisher(String, 'robot_beacon', 10)
        self.debug_merge_frontiers_pub_ = self.create_publisher(OccupancyGrid, self.robot_name_ + '/merged_frontiers_debug', 10)
        self.debug_merge_map_pub_ = self.create_publisher(OccupancyGrid, self.robot_name_ + '/merged_map_debug', 10)
        self.debug_frontier_pub_ = self.create_publisher(OccupancyGrid, self.robot_name_ + '/frontier_map_debug', 10)
        self.inflated_map_pub_ = self.create_publisher(OccupancyGrid, self.robot_name_ + '/inflated_map_debug', 10)
        
        self.init_offset_dict_ = dict()
        self.init_offset_to_current_robot_dict_ = dict()

        self.declare_parameters(
            namespace='',
            parameters=[
                ('robot_name', None),
                ('peer_list', None),
                ('simulation_mode', None),
                ('tb0_init_offset', None),
                ('tb1_init_offset', None),
                ('tb2_init_offset', None),
                ('tb3_init_offset', None),
                ('pid', None)
            ]
        )
            
            # qos_profile=qos_profile_sensor_data)
        # rclpy.spin_once(self)
        peer_list_param_name =  'peer_list'
        self.persistent_robot_peers_ = self.get_parameter(peer_list_param_name).value
        print('robot peers from param file:')
        print(self.persistent_robot_peers_)
        print(len(self.persistent_robot_peers_))
        self.e_util = ExploreUtil()
        self.map_frontier_merger_ = MapAndFrontierMerger(self.robot_name_) 
        
        self.r_interface_ = RobotControlInterface(self.robot_name_)
        self.r_interface_.debugPrint()

        # self.peer_interface_ = PeerInterfaceNode(self.robot_name_)
        self.tic_ = 0

        # self.group_coordinator_ = GroupCoordinator(self.robot_name_)

        #support function update()
        self.window_frontiers = None
        self.window_frontiers_msg = None
        self.window_frontiers_rank = None

        self.peer_state_pid_list_ = None

        self.going_to_target_failed_times_ = 0
        self.is_leader_in_current_cluster = False
        self.last_failed_frontier_pt_ = Pose()
        self.is_action_finished_ = True
        self.is_wfd_action_finished_ = True
        self.action_result_pose_ = None

    def setRobotState(self, state):
        self.current_state_ = state
        # self.peer_interface_.current_state_ = state

    def getPeerRobotInitPose(self):
        for robot in self.persistent_robot_peers_:
            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.persistent_robot_peers_:
            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



    def initRobotUtil(self):
        #some init work, discover peer robots, get their relative transforms from the local fixed frame
        init_success = False
        t_0 = time.time()
        print('total robot num:'+ str(self.total_robot_num_))
        print('persistent_robot_peers:' + str(len(self.persistent_robot_peers_)))
        while time.time() < t_0 + 10.0 and init_success == False:
            if str(len(self.persistent_robot_peers_)) == str(self.total_robot_num_):
                self.get_logger().error('get peer info, try to get transform')
                init_success = True
               
            else:
                self.get_logger().info('working in single_robot mode, failed to find peer robots')
                init_success = False
        self.getPeerRobotInitPose()

    def getMapValueOnCoordsCallback(self, request, response):
        self.get_logger().warn('getMapValueOnCoordsCallback!!!!!!!!!!!!!!!!!!!!!')
        query_pt_list = request.query_pt_list
        for query_pt in query_pt_list:
            local_query_pt = query_pt
            local_query_pt.x = query_pt.x - self.init_offset_dict_[self.robot_name_].position.x
            local_query_pt.y = query_pt.y - self.init_offset_dict_[self.robot_name_].position.y
            local_query_pt.z = query_pt.z - self.init_offset_dict_[self.robot_name_].position.z 
            local_cell_x = (int)((local_query_pt.x - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution)
            local_cell_y = (int)((local_query_pt.y - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution)
            local_cell_idx = local_cell_y*self.inflated_local_map_.info.width + local_cell_x
            response.pt_value_list.append(self.inflated_local_map_.data[local_cell_idx])

        return response

    def getLocalMapAndFrontierCompressCallback(self, request, response):
        # self.get_logger().warn('{} getLocalMapAndFrontierCompressRequest'.format(self.robot_name_))
        if self.inflated_local_map_ == None:
            self.inflated_local_map_ = OccupancyGrid()
        local_map_bytes = pickle.dumps(self.inflated_local_map_)
        # self.get_logger().warn('before compressed map size:{}'.format(len(local_map_bytes)))
        compressed_local_map_bytes = lzma.compress(local_map_bytes)
        # self.get_logger().warn('after compressed map size:{}'.format(len(compressed_local_map_bytes)))
        # print(compressed_local_map_bytes)
        
        # response.map_compress = []
        # for i in range(len(compressed_local_map_bytes)):
        #     response.map_compress.append(compressed_local_map_bytes[i])
        response.map_compress = list(compressed_local_map_bytes)
        # response.map_compress = [b'a',b'c',b'r',b'w']
        response.local_frontier = self.local_frontiers_msg_
        # self.get_logger().warn('send map time: {}'.format(time.time()))
        return response

    def getLocalMapAndFrontierCallback(self, request, response):

        self.get_logger().warn('{} getLocalMapAndFrontierRequest'.format(self.robot_name_))
        if self.inflated_local_map_ == None:
            self.inflated_local_map_ = OccupancyGrid()
        response.map = self.inflated_local_map_            
        response.local_frontier = self.local_frontiers_msg_
        self.get_logger().warn('send map time: {}'.format(time.time()))

        return response
        



    def getRobotCurrentPos(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.world_frame_, self.local_fixed_frame_,when,timeout=Duration(seconds=5.0))
            # self.get_logger().info('Got {}'.format(repr(transform)))
            self.current_pos_ = (transform.transform.translation.x, transform.transform.translation.y)
            self.current_pose_local_frame_.pose.position.x = self.current_pos_[0]
            self.current_pose_local_frame_.pose.position.y = self.current_pos_[1]
            self.current_pose_local_frame_.pose.position.z = 0.0
            self.current_pose_local_frame_.pose.orientation.x = transform.transform.rotation.x
            self.current_pose_local_frame_.pose.orientation.y = transform.transform.rotation.y
            self.current_pose_local_frame_.pose.orientation.z = transform.transform.rotation.z
            self.current_pose_local_frame_.pose.orientation.w = transform.transform.rotation.w

            

            # t_1 = time.time()
            # self.get_logger().info('(getRobotCurrentPos): robot pos:({},{}), used time:{}'.format(self.current_pos_[0], self.current_pos_[1], t_1 - t_0))
            
            return True
        except LookupException as e:
            self.get_logger().error('failed to get transform {}'.format(repr(e)))
            return False



    def localMapCallback(self, map_msg):
        #do a window_WFD from robot's current position, get a set of new frontiers, and integrate the new found frontiers with existing self.local_frontiers_
        #self.get_logger().warn('{}:localMapCallback'.format(self.tic_))
        mutex = Lock()
        mutex.acquire()
        if self.local_map_callback_lock_ == True:
            return
        self.local_map_callback_lock_ = True
        self.local_map_ = map_msg

        #self.get_logger().warn('{}:before inflateMap'.format(self.tic_))
        self.inflated_local_map_ = self.e_util.inflateMap(self.local_map_, 4)
        #self.get_logger().warn('{}:after inflateMap'.format(self.tic_))
        self.inflated_map_pub_.publish(self.local_map_)
        self.local_map_callback_lock_ = False
        self.tic_ = self.tic_ + 1
        mutex.release()

        
    #return 
    # -1 : no inflated_local_map
    # -2 : can not get robot current pose from tf
    # temp_window_frontiers_ , if empty, then no window_frontiers detected
    def getWindowFrontiers(self):
        mutex = Lock()
        mutex.acquire()
        temp_window_frontiers_ = []
        temp_window_frontiers_msg_ = []
        if self.inflated_local_map_ == None or self.inflated_local_map_ == OccupancyGrid():
            self.get_logger().error('(getWindowFrontiers): no inflated_local_map')
            return -1
        self.get_logger().info('(getWindowFrontiers): init')
        if self.getRobotCurrentPos():
            # self.get_logger().warn('(updateLocalFrontiersUsingWindowWFD): start window_wfd!!!')
            current_map = OccupancyGrid()
            current_map.header = self.inflated_local_map_.header
            current_map.info = self.inflated_local_map_.info
            current_map.data = list(self.inflated_local_map_.data)
            window_wfd = WindowWFD(current_map, self.current_pos_, 150)
            # if self.DEBUG_ == True:
                # self.inflated_map_pub_.publish(current_map)
            t_0 = time.time()
            window_frontiers_cell, covered_set = window_wfd.getWindowFrontiers()
            t_1 = time.time()

            # transform local_frontiers from the cell format to absolute position in 'double', because the cell could change due to map expansion
            
            self.get_logger().info('(getWindowFrontiers)window_frontiers_cell size:{}, used time:{}'.format(len(window_frontiers_cell), t_1 - t_0))
            for f_connect_cell in window_frontiers_cell:
                f_connect = []
                f_msg = Frontier()
                for f_cell in f_connect_cell:
                    f_double = (f_cell[0]*current_map.info.resolution + current_map.info.origin.position.x, f_cell[1]*current_map.info.resolution + current_map.info.origin.position.y)
                    f_connect.append(f_double)
                    pt_stamped = PointStamped()
                    pt_stamped.header = current_map.header
                    pt_stamped.point.x = f_double[0]
                    pt_stamped.point.y = f_double[1]
                    pt_stamped.point.z = 0.0
                    f_msg.frontier.append(pt_stamped)
                temp_window_frontiers_msg_.append(f_msg)
                temp_window_frontiers_.append(f_connect)
        else:
            self.get_logger().error('(getWindowFrontiers): failed to get robot current pos')
            return -2
        mutex.release()
        return temp_window_frontiers_

    #detect window_WFD and integrate with local frontiers, will update self.local_frontiers_
    def updateLocalFrontiersUsingWindowWFD(self):
        mutex = Lock()
        mutex.acquire()
        temp_window_frontiers_ = []
        temp_window_frontiers_msg_ = []
        temp_window_frontiers_rank_ = []
        if self.inflated_local_map_ == None or self.inflated_local_map_ == OccupancyGrid():
            self.get_logger().error('(updateLocalFrontiersUsingWindowWFD): no inflated_local_map')
            return -1, -1, -1
        # self.get_logger().info('(updateLocalFrontiersUsingWindowWFD): init')
        if self.getRobotCurrentPos():
            # self.get_logger().warn('(updateLocalFrontiersUsingWindowWFD): start window_wfd!!!')
            current_map = OccupancyGrid()
            current_map.header = self.inflated_local_map_.header
            current_map.info = self.inflated_local_map_.info
            current_map.data = list(self.inflated_local_map_.data)
            window_wfd = WindowWFD(current_map, self.current_pos_, 250)
            # if self.DEBUG_ == True:
                # self.inflated_map_pub_.publish(current_map)
            try:
                t_0 = time.time()
            except:
                print("unexcepted error:", sys.exc_info()[0])
            window_frontiers_cell, covered_set = window_wfd.getWindowFrontiers()
            t_1 = time.time()

            # transform local_frontiers from the cell format to absolute position in 'double', because the cell could change due to map expansion
            
            self.get_logger().info('(updateLocalFrontiersUsingWindowWFD)window_frontiers_cell size:{}, used time:{}'.format(len(window_frontiers_cell), t_1 - t_0))
            for f_connect_cell in window_frontiers_cell:
                f_connect = []
                f_msg = Frontier()
                if len(f_connect_cell)<6:
                    continue
                for f_cell in f_connect_cell:
                    f_double = (f_cell[0]*current_map.info.resolution + current_map.info.origin.position.x, f_cell[1]*current_map.info.resolution + current_map.info.origin.position.y)
                    f_connect.append(f_double)
                    pt_stamped = PointStamped()
                    pt_stamped.header = current_map.header
                    pt_stamped.point.x = f_double[0]
                    pt_stamped.point.y = f_double[1]
                    pt_stamped.point.z = 0.0
                    f_msg.frontier.append(pt_stamped)
                temp_window_frontiers_msg_.append(f_msg)
                temp_window_frontiers_.append(f_connect)
                temp_window_frontiers_rank_.append(f_connect_cell[0][2])


            #update self.local_frontiers_msg and self.local_frontiers_ with temp_window_frontiers_ , temp_window_frontiers_msg_
            self.updateLocalFrontiers(temp_window_frontiers_, temp_window_frontiers_msg_, window_wfd.window_size_, self.current_pos_, current_map, covered_set)

            #DEBUG
            if self.DEBUG_:
                # self.get_logger().warn('debug local_frontiers')
                local_map_dw = current_map.info.width
                local_map_dh = current_map.info.height
                frontiers_index_list = []
                # for f_connect in window_frontiers_cell:
                #     for f_cell in f_connect:
                #         frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
                for f_connect in self.local_frontiers_:
                    for f_double in f_connect:
                        f_cell = ((int)((f_double[0] - current_map.info.origin.position.x) / current_map.info.resolution) ,  (int)((f_double[1] - current_map.info.origin.position.y) / current_map.info.resolution))
                        frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
                frontier_debug_map = OccupancyGrid()
                frontier_debug_map.header = current_map.header
                frontier_debug_map.info = current_map.info

                temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
                temp_array[:]=(int)(-1)
                frontier_debug_map.data = temp_array.ravel().tolist()
                for idx in frontiers_index_list:
                    if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
                        continue
                    frontier_debug_map.data[int(idx)] = 0
                frontier_map_width = frontier_debug_map.info.width
                frontier_map_height = frontier_debug_map.info.height



                self.debug_frontier_pub_.publish(frontier_debug_map)
                pass
                # self.get_logger().warn('end debug local_frontiers')
                #DEBUG

        else:
            self.get_logger().error('(updateLocalFrontiersUsingWindowWFD): failed to get robot current pos')
            return -2, -2, -2
        self.get_logger().info('(updateLocalFrontiersUsingWindowWFD): end')
        mutex.release()
        return temp_window_frontiers_, temp_window_frontiers_msg_, temp_window_frontiers_rank_

    def generateFrontierDebugMap(self, frontier_msg_list, current_map):
        local_map_dw = current_map.info.width
        local_map_dh = current_map.info.height
        frontiers_index_list_w = []
        frontiers_index_list_h = []

        # for f_connect in local_frontiers_cell:
        #     for f_cell in f_connect:
        #         frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0])
        for f_connect_msg in frontier_msg_list:
            for f_pt in f_connect_msg.frontier:
                f_cell = ((int)((f_pt.point.x - current_map.info.origin.position.x) / current_map.info.resolution) ,  (int)((f_pt.point.y - current_map.info.origin.position.y) / current_map.info.resolution))
                frontiers_index_list_h.append(f_cell[1])
                frontiers_index_list_w.append(f_cell[0])

        frontier_debug_map = OccupancyGrid()
        frontier_debug_map.header = copy.deepcopy(current_map.header)
        frontier_debug_map.info = copy.deepcopy(current_map.info)

        
        temp_array = np.asarray(current_map.data, dtype=np.int8).reshape(local_map_dh, local_map_dw)
        temp_array[:]=(int)(-1)
        temp_array[frontiers_index_list_h, frontiers_index_list_w] = 0
        # temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8)
        frontier_debug_map.data = temp_array.ravel().tolist()
        # for idx in frontiers_index_list:
        #     if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1:
        #         continue
        #     frontier_debug_map.data[int(idx)] = 0
        # frontier_map_width = frontier_debug_map.info.width
        # frontier_map_height = frontier_debug_map.info.height
        return frontier_debug_map



    def updateLocalFrontiers(self, new_frontiers, new_frontiers_msg, window_size, current_pos, map, covered_set):
        #update self.local_frontiers_ , self.local_frontiers_msg_ , they could be empty or already have some frontiers
        if len(self.local_frontiers_) == 0:
            self.local_frontiers_ = new_frontiers
            self.local_frontiers_msg_ = new_frontiers_msg
        else:

            # print('self.local_frontiers_ size:{}'.format(len(self.local_frontiers_)))
            # print('self.local_frontiers_msg_ size:{}'.format(len(self.local_frontiers_msg_)))
            copied_local_frontiers_ = list(self.local_frontiers_)
            copied_local_frontiers_msg_ = list(self.local_frontiers_msg_)
            for old_index, old_f in enumerate(copied_local_frontiers_):


                
                if self.e_util.isFrontierWithinWindow(old_f, current_pos, window_size * map.info.resolution - 0.5, map, covered_set) or self.e_util.isFrontierWithinObs(old_f, current_pos, window_size * map.info.resolution - 0.5, map, covered_set):
                    self.local_frontiers_.remove(old_f)
                    delete_f_msg = copied_local_frontiers_msg_[old_index]
                    self.local_frontiers_msg_.remove(delete_f_msg)
            
            
            for new_f in new_frontiers:
                self.local_frontiers_.append(new_f)
            for new_f_msg in new_frontiers_msg:
                self.local_frontiers_msg_.append(new_f_msg)

   



    def updateMultiHierarchicalCoordination(self):
        #state of current robot: GOING_TO_TARGET, WAITING_NEW_TARGET, REACH_TARGET, FINISH_TARGET_WINDOW_NOT_DONE, 
        if self.current_state_ == self.e_util.SYSTEM_INIT:
            #robot rotate inplace
            self.r_interface_.rotateNCircles(1, 0.4)
            # self.updateWindowWFD()
            self.r_interface_.stopAtPlace()
            #self.current_state_ = self.TEST_MERGE_MAP
            self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
        elif self.current_state_ == self.e_util.GOING_TO_TARGET:
            self.get_logger().error('updateMulti: enter GOING_TO_TARGET')
            
            if self.current_target_pos_ == None:
                self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
                return
            self.get_logger().warn('go to target:({},{}), orientation({},{},{},{})'.format(self.current_target_pos_.position.x, self.current_target_pos_.position.y, self.current_target_pos_.orientation.x, self.current_target_pos_.orientation.y, self.current_target_pos_.orientation.z, self.current_target_pos_.orientation.w))
            self.r_interface_.navigateToPoseFunction(self.current_target_pos_)
            # self.getRobotCurrentPos()
            # direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
            is_thread_started = False
            check_environment_thread = Thread(target=self.checkEnvironmentFunction)
            while self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_MOVING:

                # self.get_logger().error('start checking environment...')
                # self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
                self.getRobotCurrentPos()
                current_direct_length_square = (self.current_pos_[0] - self.current_target_pos_.position.x)*(self.current_pos_[0] - self.current_target_pos_.position.x) + (self.current_pos_[1] - self.current_target_pos_.position.y)*(self.current_pos_[1] - self.current_target_pos_.position.y)
                # self.get_logger().warn("navigating to target {},{}".format(self.current_target_pos_.position.x, self.current_target_pos_.position.y ))
                # current_direct_length_square = 10.0
                if current_direct_length_square < 2.0*2.0:
                    if is_thread_started == False:
                        check_environment_thread.start()

                        is_thread_started = True
                    # return
                    
                pass
            if self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_DONE:
                self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
            elif self.r_interface_.navigate_to_pose_state_ == self.e_util.NAVIGATION_FAILED:
                self.get_logger().error('NAVIGATION_FAILED')

                current_target_cell = ((int)((self.current_target_pos_.position.x - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) ,  (int)((self.current_target_pos_.position.y - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
                # self.getRobotCurrentPos()
                # current_cell = ((int)((self.current_pos_[0] - self.inflated_local_map_.info.origin.position.x) / self.inflated_local_map_.info.resolution) ,  (int)((self.current_pos_[1] - self.inflated_local_map_.info.origin.position.y) / self.inflated_local_map_.info.resolution))
                updated_cell = self.e_util.getFreeNeighborRandom(current_target_cell, self.inflated_local_map_, 30, 50)
                if updated_cell == None:
                    self.get_logger().warn('updated_cell is None, CHECK_ENVIRONMENT now!!!!!')
                    self.setRobotState(self.e_util.CHECK_ENVIRONMENT) 
                    return
                updated_target_pt = (updated_cell[0]*self.inflated_local_map_.info.resolution + self.inflated_local_map_.info.origin.position.x, updated_cell[1]*self.inflated_local_map_.info.resolution + self.inflated_local_map_.info.origin.position.y)
                self.current_target_pos_.position.x = updated_target_pt[0]
                self.current_target_pos_.position.y = updated_target_pt[1]


                if self.previous_state_ != self.e_util.GOING_TO_TARGET:
                    self.get_logger().error('going_to_target_failed_times_ = 000000000000')
                    self.going_to_target_failed_times_ = 0
                else:
                    self.going_to_target_failed_times_ += 1
                    self.get_logger().error('going_to_target_failed_times_ = {}{}{}{}{}'.format(self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_,self.going_to_target_failed_times_))

                self.previous_state_ = self.current_state_ 
                if self.going_to_target_failed_times_ > 10:
                    self.get_logger().warn('going_to_target_failed_times_ > 10')
                    self.get_logger().warn('going_to_target_failed_times_ > 10')
                    self.get_logger().warn('going_to_target_failed_times_ > 10')
                    self.get_logger().warn('going_to_target_failed_times_ > 10')
                    self.get_logger().warn('going_to_target_failed_times_ > 10')
                    self.last_failed_frontier_pt_ = self.current_target_pos_
                    self.going_to_target_failed_times_ = 0 
                    self.setRobotState(self.e_util.CHECK_ENVIRONMENT) 
                else:
                    self.get_logger().error('retry same target again!!!!!!!!!!!!!!!!')
                    self.setRobotState(self.e_util.GOING_TO_TARGET) 


            if is_thread_started == True:
                check_environment_thread.join()
                self.current_target_pos_ = self.next_target_pos_
                is_thread_started == False
                self.setRobotState(self.e_util.GOING_TO_TARGET) 



            # self.setRobotState(self.e_util.CHECK_ENVIRONMENT)
        elif self.current_state_ == self.e_util.CHECK_ENVIRONMENT:
            self.get_logger().error('Enter CHECK_ENVIRONMENT')
            
            #check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
            #if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE

            wfd_response = self.send_wfd_service_request()
            # self.send_wfd_action_goal()
            # while self.is_wfd_action_finished_ != True:
            #     pass 
            self.local_frontiers_msg_ = wfd_response.local_frontiers
            self.window_frontiers_msg = wfd_response.window_frontiers  
            self.window_frontiers_rank = wfd_response.window_frontiers_rank

            # self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
            # if self.window_frontiers == -1 or self.window_frontiers == -2:
            #     self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
            #     self.current_state_ = self.e_util.CHECK_ENVIRONMENT
            # else:   
                

            self.send_group_coordinator_goal()
            while self.is_action_finished_ != True:
                pass


            self.current_target_pos_ = self.action_result_pose_


            # self.group_coordinator_.setPeerInfo(self.persistent_robot_peers_, peer_pose_dict, cluster_list, cluster_pose_dict, self.window_frontiers, self.window_frontiers_rank, self.local_frontiers_, self.local_frontiers_msg_, self.inflated_local_map_, self.peer_interface_.init_offset_dict_, self.last_failed_frontier_pt_)
            # self.current_target_pos_ = self.group_coordinator_.hierarchicalCoordinationAssignment()
            if self.current_target_pos_ == None:
                self.get_logger().error('finish local_frontiers, done for current robot')
                self.setRobotState(self.e_util.FINISH_TASK)
            else:
                self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
                self.setRobotState(self.e_util.GOING_TO_TARGET)
                
                    # no window frontiers available, find target frontier from self.local_frontiers_msg_, considering distance from peer robot_tracks 
        elif self.current_state_ == self.e_util.FINISH_TASK:
            self.r_interface_.rotateNCircles(1, 0.4)
            self.get_logger().error('finish local_frontiers, done for current robot')


    def send_group_coordinator_goal(self):
        self._action_client.wait_for_server()
        while self.is_action_finished_ == False:
            pass
        self.is_action_finished_ = False
        goal_msg = GroupCoordinator.Goal()
        for peer in self.persistent_robot_peers_:
            peer_msg = String()
            if peer != self.robot_name_:
                peer_msg.data = peer
                goal_msg.peer_list.append(peer_msg)
        if self.getRobotCurrentPos():
            goal_msg.robot_pose_local_frame = self.current_pose_local_frame_
        else:
            self.get_logger().error('(send_group_coordinator_goal) fail to get robot current pose')
            return
        goal_msg.window_frontiers = self.window_frontiers_msg
        goal_msg.window_frontiers_rank = self.window_frontiers_rank
        goal_msg.local_frontiers = self.local_frontiers_msg_
        goal_msg.local_inflated_map = self.inflated_local_map_
        goal_msg.last_failed_frontier_pt.pose = self.last_failed_frontier_pt_ 
        # goal_uuid = 0
        # if self.robot_name_ == 'tb0':
        #     goal_uuid = 0
        # elif self.robot_name_ == 'tb1':
        #     goal_uuid = 1
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_group_coordinator_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def feedback_group_coordinator_callback(self, feedback):
        pass

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('group coordinator goal rejected')
            return 
        
        self.get_logger().warn('group coordinator goal accepted')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
            self.action_result_pose_ = result.current_target_pose
            self.is_action_finished_ = True
        else:
            self.get_logger().error('Goal failed with status: {}'.format(status))

    def send_wfd_service_request(self):
        while not self.wfd_service_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('wfd servie not available,wait')
        req = WfdService.Request()
        if self.getRobotCurrentPos():
            req.robot_pose_local_frame = self.current_pose_local_frame_.pose
        req.local_frontiers = self.local_frontiers_msg_
        req.local_inflated_map = self.inflated_local_map_
        return self.wfd_service_client.call(req)
        

    def send_wfd_action_goal(self):
        self.wfd_action_client.wait_for_server()
        while self.is_wfd_action_finished_ == False:
            pass
        self.is_wfd_action_finished_ = False
        goal_msg = WfdAction.Goal()
        goal_msg.local_frontiers = self.local_frontiers_msg_
        if self.getRobotCurrentPos():
            goal_msg.robot_pose_local_frame = self.current_pose_local_frame_.pose
        else:
            self.get_logger().error('(send_wfd_goal) fail to get robot current pose')
            return        
        goal_msg.local_inflated_map = self.inflated_local_map_

        self._send_wfd_goal_future = self.wfd_action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_wfd_action_callback)

        self._send_wfd_goal_future.add_done_callback(self.wfd_goal_response_callback)

    def feedback_wfd_action_callback(self, feedback):
        pass

    def wfd_goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error('wfd goal rejected')
            return 
        
        self.get_logger().warn('wfd goal accepted')
        self._get_wfd_result_future = goal_handle.get_result_async()
        self._get_wfd_result_future.add_done_callback(self.get_wfd_result_callback)

    def get_wfd_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
            self.local_frontiers_msg_ = result.local_frontiers
            self.window_frontiers_msg = result.window_frontiers  
            self.window_frontiers_rank = result.window_frontiers_rank
            # self.window_frontiers_.clear()
            # for wf_msg in self.window_frontiers_msg:
                
            self.is_wfd_action_finished_ = True
        else:
            self.get_logger().error('Goal failed with status: {}'.format(status))


    def checkEnvironmentFunction(self):
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        self.get_logger().error('Enter CHECK_ENVIRONMENT_THREAD')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')

            
        #check current window_frontiers, if window_frontiers is empty, then FINISH_TARGET_WINDOW_DONE
        #if window_frontiers is not empty, then FINISH_TARGET_WINDOW_NOT_DONE
        # self.send_wfd_action_goal()
        # while self.is_wfd_action_finished_ != True:
        #     pass 
        wfd_response = self.send_wfd_service_request()
        # self.send_wfd_action_goal()
        # while self.is_wfd_action_finished_ != True:
        #     pass 
        self.local_frontiers_msg_ = wfd_response.local_frontiers
        self.window_frontiers_msg = wfd_response.window_frontiers  
        self.window_frontiers_rank = wfd_response.window_frontiers_rank


        # self.window_frontiers, self.window_frontiers_msg, self.window_frontiers_rank = self.updateLocalFrontiersUsingWindowWFD()
        # if self.window_frontiers == -1 or self.window_frontiers == -2:
        #     self.get_logger().error('(update.CHECK_ENVIRONMENT) failed getting WindowFrontiers, check robot, something is wrong')
        #     #self.current_state_ = self.e_util.CHECK_ENVIRONMENT
        # else:   
        self.send_group_coordinator_goal()
        while self.is_action_finished_ != True:
            pass


        self.next_target_pos_ = self.action_result_pose_


            
            
            
            # if self.current_target_pos_ == None:
            #     self.get_logger().error('finish local_frontiers, done for current robot')
            #     self.setRobotState(self.e_util.FINISH_TASK)
            # else:
            #     self.previous_state_ = self.e_util.CHECK_ENVIRONMENT
            #     self.setRobotState(self.e_util.GOING_TO_TARGET)
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        # self.get_logger().warn('$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ ')
        self.get_logger().error('EXIT CHECK_ENVIRONMENT_THREAD, next target:{},{}'.format(self.next_target_pos_.position.x, self.next_target_pos_.position.y))
예제 #20
0
class AutoMap(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self.client = ActionClient(self, NavigateToPose, "NavGoal")
        self.goal_nbr = 0
        self.goal = None
        self.robot_pose_img = Point()
        self.target = Point()
        self.call = None
        self.map_data = None
        self.width = 0
        self.height = 0
        self.map_origin = Point()
        self.resolution = 0
        self.robot_yaw = 0
        self.robot_pose = Point()
        self.tf_buffer = Buffer()
        self.tf_timer = None
        self.listener = TransformListener(self.tf_buffer, self)
        try:
            self.map_srv = self.create_client(GetMap, "/map_server/map")
            self.get_logger().info("Service client created")
            while not self.map_srv.wait_for_service(timeout_sec=1.0):
                self.get_logger().info(
                    'service not available, waiting again...')
        except Exception as e:
            self.get_logger().info(e)
        self.get_logger().info("robot initialization finished")

    @staticmethod
    def quaternion_to_euler(quaternion):
        t0 = +2.0 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z)
        t1 = +1.0 - 2.0 * (quaternion.x * quaternion.x +
                           quaternion.y * quaternion.y)
        roll = atan2(t0, t1)
        t2 = +2.0 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        pitch = asin(t2)
        t3 = +2.0 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y)
        t4 = +1.0 - 2.0 * (quaternion.y * quaternion.y +
                           quaternion.z * quaternion.z)
        yaw = atan2(t3, t4)
        return [yaw, pitch, roll]

    @staticmethod
    def euler_to_quaternion(roll, pitch, yaw):
        qx = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - cos(
            roll / 2) * sin(pitch / 2) * sin(yaw / 2)
        qy = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + sin(
            roll / 2) * cos(pitch / 2) * sin(yaw / 2)
        qz = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - sin(
            roll / 2) * sin(pitch / 2) * cos(yaw / 2)
        qw = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + sin(
            roll / 2) * sin(pitch / 2) * sin(yaw / 2)
        return [qx, qy, qz, qw]

    def get_robot_pose(self):
        try:
            tf = self.tf_buffer.lookup_transform("base_link", "base_link",
                                                 Time())
            self.robot_pose.x = tf.transform.translation.x
            self.robot_pose.y = tf.transform.translation.y
            robot_rot = tf.transform.rotation
            self.robot_pose.z = self.quaternion_to_euler(robot_rot)[2]
        except LookupException:
            self.get_logger().info('transform not ready')

    def request_map(self):
        self.get_logger().info("Requesting GetMap data")
        self.call = self.map_srv.call_async(GetMap.Request())
        self.call.add_done_callback(self.get_map)

    def get_map(self, response):
        try:
            # response: GetMap.Response = self.call.result()
            self.get_logger().info("GetMap data received")
            response_map = response.result().map
            self.width = response_map.info.width
            self.height = response_map.info.height
            self.resolution = response_map.info.resolution
            origin_yaw = self.quaternion_to_euler(
                response_map.info.origin.orientation)[2]
            origin_x = response_map.info.origin.position.x
            origin_y = response_map.info.origin.position.y
            self.map_origin = Point(x=origin_x, y=origin_y, z=origin_yaw)
            self.tf_timer = self.create_timer(1.0, self.get_robot_pose)
            self.map_data = list(response_map.data)
            for i in range(self.height):
                for j in range(self.width):
                    self.map_data[i * self.width + j] = response_map.data[
                        (self.height - 1 - i) * self.width + j]
            self.pose_to_pix()
            self.check_directions()
            if not self.find_next_goal():
                self.get_logger().info("No new goal found")
                return
            self.pix_to_pose()
            self.send_target()
        except Exception as e:
            self.get_logger().info('Service call failed %r' % (e, ))

    def pose_to_pix(self):
        self.robot_pose_img.x = float(
            round((self.map_origin.y - self.robot_pose.y) / self.resolution +
                  self.height))
        self.robot_pose_img.y = float(
            round((self.robot_pose.x - self.map_origin.x) / self.resolution))
        self.robot_pose_img.z = float(
            round(self.robot_pose.z - self.map_origin.z))

    def pix_to_pose(self):
        self.target.x = self.robot_pose_img.y * self.resolution + self.map_origin.x
        self.target.y = -(
            (self.robot_pose_img.x - self.height) * self.resolution -
            self.map_origin.y)
        self.target.z = self.robot_pose_img.z + self.map_origin.z

    def is_accessible(self, x, y, safe_dist=4):
        if self.map_data[int(x * self.width + y)] == 0:
            for m in range(-safe_dist, safe_dist + 1):
                for n in range(-safe_dist, safe_dist + 1):
                    if self.map_data[int((x + m) * self.width +
                                         (y + n))] == 100:
                        return False
            return True
        return False

    def is_free(self, x, y):
        if self.map_data[int(x * self.width + y)] == -2:
            for k in range(-1, 2):
                for l in range(-1, 2):  # On regarde les cellules adjacentes
                    if k != 0 or l != 0:
                        if self.map_data[int(
                            (x + k) * self.width + (y + l)
                        )] == -1:  # Si une de ces cellules adjacentes est inconnue
                            return True
                    return False
        else:
            return False

    def check_directions(self):
        pile = []
        for i in range(-3, 4):
            for j in range(-3, 4):
                pile.append([
                    int(self.robot_pose_img.x) + i,
                    int(self.robot_pose_img.y) + j
                ])
        while pile:
            [x, y] = pile.pop()
            self.map_data[int(x * self.width + y)] = -2
            for k in range(-1, 2):
                for l in range(-1, 2):
                    if k != 0 or l != 0:
                        if self.is_accessible(x + k, y + l):
                            pile.append([x + k, y + l])

    def find_next_goal(self, rayon=10):
        while 2 * rayon < max(self.height, self.width):
            for i in range(-rayon, rayon + 1):
                if i == -rayon or i == rayon:
                    for j in range(-rayon, rayon + 1):
                        if self.robot_pose_img.x + i < self.width and self.robot_pose_img.y + j < self.height:
                            if self.is_free(self.robot_pose_img.x + i,
                                            self.robot_pose_img.y + j):
                                self.target.x = self.robot_pose_img.x + i
                                self.target.y = self.robot_pose_img.y + j
                                return True
                else:
                    if self.robot_pose_img.x + i < self.width and self.robot_pose_img.y + j < self.height:
                        if self.is_free(self.robot_pose_img.x + i,
                                        self.robot_pose_img.y + rayon):
                            self.target.x = self.robot_pose_img.x + i
                            self.target.y = self.robot_pose_img.y + rayon
                            return True
                        if self.is_free(self.robot_pose_img.x + i,
                                        self.robot_pose_img.y - rayon):
                            self.target.x = self.robot_pose_img.x + i
                            self.target.y = self.robot_pose_img.y - rayon
                            return True
            rayon = rayon + 1
        return False

    def goal_reached(self, resp):
        goal_handle = resp.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return
        self.request_map()

    def send_target(self):
        quaternion = self.euler_to_quaternion(0, 0, self.target.z)
        goal = NavigateToPose.Goal()
        self.goal_nbr = self.goal_nbr + 1
        goal.pose.header.seq = self.goal_nbr
        goal.pose.header.stamp = Time()
        goal.pose.header.frame_id = "sendGoal"
        goal.pose.pose.position = Point(x=self.target.x,
                                        y=self.target.y,
                                        yaw=0)
        goal.pose.pose.orientation.x = quaternion[0]
        goal.pose.pose.orientation.y = quaternion[1]
        goal.pose.pose.orientation.z = quaternion[2]
        goal.pose.pose.orientation.w = quaternion[3]
        self.goal = self.client.send_goal_async(goal)
        self.goal.add_done_callback(self.goal_reached)
예제 #21
0
class WIGGLE(object):
    def __init__(self, save=True, cycle_time=.5, steps=20):
        self.steps = steps
        self.steps_size = 2 * np.pi / self.steps
        self.change_cycle_time(cycle_time)
        self.is_giskard_active = False

        self.giskard_goals = rospy.Publisher('/whole_body_controller/goal',
                                             WholeBodyCommand,
                                             queue_size=10)
        self.pose_pub = rospy.Publisher('/test', PoseStamped, queue_size=10)
        self.wiggle_counter = 0.
        self.tfBuffer = Buffer()
        self.tf = TransformListener(self.tfBuffer)
        self.save = save

        self.wiggle_action_server = actionlib.SimpleActionServer(
            'wiggle_wiggle_wiggle',
            WiggleAction,
            execute_cb=self.wiggle_cb,
            auto_start=False)
        self.wiggle_action_server.start()
        self.giskard_status = rospy.Subscriber(
            '/controller_action_server/move/status',
            GoalStatusArray,
            self.giskard_status_cb,
            queue_size=10)
        rospy.sleep(1.)

    def giskard_status_cb(self, data):
        if len(data.status_list) > 0:
            self.is_giskard_active = data.status_list[
                -1].status == GoalStatus.ACTIVE

    def change_cycle_time(self, new_cycle_time):
        self.rate = int(self.steps / new_cycle_time)

    def transformPose(self, target_frame, pose):
        transform = self.tfBuffer.lookup_transform(
            target_frame,
            pose.header.frame_id,  # source frame
            rospy.Time(0),  # get the tf at first available time
            rospy.Duration(1.0))
        new_pose = do_transform_pose(pose, transform)
        return new_pose

    def wiggle_cb(self, goal):
        # check for invalid goals
        if goal.arm != WiggleGoal.LEFT_ARM and goal.arm != WiggleGoal.RIGHT_ARM:
            rospy.logerr('arm goal should be {} or {}'.format(
                WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
            return self.wiggle_action_server.set_aborted(
                text='arm goal should be {} or {}'.format(
                    WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
        if np.linalg.norm(
                np.array([
                    goal.goal_pose.pose.orientation.x,
                    goal.goal_pose.pose.orientation.y,
                    goal.goal_pose.pose.orientation.z,
                    goal.goal_pose.pose.orientation.w
                ])) < 0.99:
            rospy.logerr('invalid orientation in goal position')
            return self.wiggle_action_server.set_aborted(
                text='invalid orientation in goal position')

        rospy.loginfo('wiggle wiggle wiggle')
        movement_duration = goal.timeout.data.to_sec()
        wiggle_per_sec = goal.cycle_time
        number_of_wiggles = int(movement_duration / wiggle_per_sec)
        number_of_wiggle_points = 10
        radius_x = goal.upperbound_x
        radius_y = goal.upperbound_y
        hz = number_of_wiggle_points / wiggle_per_sec

        r = rospy.Rate(hz)

        tcp_goal_pose = self.transformPose('left_gripper_tool_frame',
                                           goal.goal_pose)
        spiral = self.create_spiral(goal_height=tcp_goal_pose.pose.position.z,
                                    radius_x=radius_x,
                                    radius_y=radius_y,
                                    number_of_points=number_of_wiggle_points,
                                    number_of_wiggles=number_of_wiggles)
        header = Header()
        header.frame_id = 'left_gripper_tool_frame'
        pose_list = self.np_array_to_pose_stampeds(spiral,
                                                   Quaternion(0, 0, 0, 1),
                                                   header)
        pose_list = [
            deepcopy(self.transformPose('base_footprint', pose))
            for pose in pose_list
        ]

        end_pose = self.transformPose('base_footprint', goal.goal_pose)

        for i, pose in enumerate(pose_list):
            if self.check_for_stop():
                return
            self.pub_wiggle(pose, goal.arm)
            wiggle_feedback = WiggleFeedback()
            wiggle_feedback.progress = .9 * (i + 1) / float(len(pose_list))
            self.wiggle_action_server.publish_feedback(wiggle_feedback)
            r.sleep()

        # move back to real goal
        time_to_get_to_endpose = rospy.Duration(1.0)
        deadline = rospy.get_rostime() + time_to_get_to_endpose
        while rospy.get_rostime() < deadline:
            if self.check_for_stop():
                return
            self.pub_wiggle(end_pose, goal.arm)

        result = WiggleFeedback()
        result.progress = 1.
        self.wiggle_action_server.publish_feedback(result)
        self.wiggle_action_server.set_succeeded(result)

    def check_for_stop(self):
        if self.wiggle_action_server.is_preempt_requested():
            self.wiggle_action_server.set_preempted(WiggleFeedback(),
                                                    'goal canceled')
            return True
        if self.is_giskard_active:
            self.wiggle_action_server.set_aborted(
                WiggleFeedback(),
                'goal canceled because giskard action server is doing stuff')
            return True
        return False

    def pub_wiggle(self, goal_pose, arm):
        if self.save:
            self.wiggle_wiggle_wiggle(goal_pose, arm)
        self.pose_pub.publish(goal_pose)

    def goal_pose_to_cmd(self, goal_pose, arm):
        moving_arm_cmd = ArmCommand()
        moving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
        moving_arm_cmd.goal_pose = goal_pose
        asdf = SemanticFloat64()
        asdf.semantics = 'l_rot_error'
        asdf.value = 0.0174
        moving_arm_cmd.convergence_thresholds.append(asdf)
        asdf = SemanticFloat64()
        asdf.semantics = 'l_trans_error'
        asdf.value = 0.001
        moving_arm_cmd.convergence_thresholds.append(asdf)

        nonmoving_arm_cmd = ArmCommand()
        right_goal = PoseStamped()
        right_goal.header.frame_id = 'right_gripper_tool_frame'
        right_goal.pose.orientation.w = 1
        nonmoving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
        nonmoving_arm_cmd.goal_pose = self.transformPose(
            'base_footprint', right_goal)

        cmd = WholeBodyCommand()
        cmd.type = WholeBodyCommand.STANDARD_CONTROLLER
        if arm == WiggleGoal.LEFT_ARM:
            cmd.left_ee = moving_arm_cmd
            cmd.right_ee = nonmoving_arm_cmd
        else:
            cmd.left_ee = nonmoving_arm_cmd
            cmd.right_ee = moving_arm_cmd
        return cmd

    def wiggle_wiggle_wiggle(self, goal_pose, arm):
        if arm == WiggleGoal.LEFT_ARM or arm == WiggleGoal.RIGHT_ARM:
            cmd = self.goal_pose_to_cmd(goal_pose, arm)
            self.giskard_goals.publish(cmd)
        else:
            rospy.logerr('arm goal should be {} or {}'.format(
                WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))

    def create_spiral(self,
                      goal_height=2.,
                      radius_x=1.,
                      radius_y=1.,
                      number_of_points=10,
                      number_of_wiggles=10):
        wiggle_height = goal_height / number_of_wiggles
        wiggles = []
        wiggle_template = self.spiral_round(radius_x, radius_y, wiggle_height,
                                            number_of_points)
        for i in range(number_of_wiggles):
            wiggles.append(wiggle_template +
                           np.array([0, 0, i * wiggle_height]))
        spiral = np.concatenate(wiggles)
        return spiral

    def spiral_round(self, radius_x, radius_y, height, number_of_points):
        points = []
        tau = 2 * np.pi
        for i, r in enumerate(np.arange(0, tau, tau / number_of_points)):
            points.append(
                np.array([
                    np.sin(r) * radius_x,
                    np.cos(r) * radius_y,
                    i / float(number_of_points) * float(height)
                ]))
        return np.array(points)

    def np_array_to_pose_stampeds(self, nparray, orientation, header):
        pose_list = []
        for p in nparray:
            pose = PoseStamped()
            pose.pose.position = Point(*p)
            pose.pose.orientation = orientation
            pose.header = header
            pose_list.append(pose)
        return pose_list