Task2/factory_alignment_demo/scripts/factory_search_point_adapter.py

#!/usr/bin/env python3
"""Adapt a nominal factory-search pose into obstacle-aware move_base candidates."""
from __future__ import annotations

import math
import queue
import threading
import time

import actionlib
import rospy
import tf2_ros
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import Pose, PoseArray, PoseStamped, Quaternion
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import OccupancyGrid
from nav_msgs.srv import GetPlan, GetPlanRequest
from std_msgs.msg import String
from std_srvs.srv import Trigger, TriggerResponse


class _PlanTask:
    """One make_plan request handled by the sole background RPC worker."""

    def __init__(self, request):
        self.request = request
        self.response = None
        self.error = None
        self.finished = threading.Event()


class FactorySearchPointAdapter:
    """Send only safe observation candidates; never publish cmd_vel."""

    _NAV_FAILURE_STATES = {
        GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED,
        GoalStatus.RECALLED, GoalStatus.LOST,
    }
    _SEARCH_RETRY_PREFIXES = (
        "FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN",
        "ALIGNMENT_TIMEOUT",
        "WALL_NORMAL_UNAVAILABLE",
        "WALL_RAY_NO_HIT",
        "WALL_GOAL_INVALID_STANDOFF",
        "WALL_GOAL_OUTSIDE_MAP",
        "WALL_GOAL_OCCUPIED",
        "WALL_GOAL_MOVE_BASE_UNAVAILABLE",
        "WALL_APPROACH_FAILED",
        "FACTORY_OCR_FAILED",
        "FACTORY_ENTRY_FAILED",
    )

    def __init__(self):
        rospy.init_node("factory_search_point_adapter")
        self._frame_id = rospy.get_param("~frame_id", "map")
        self._base_frame = rospy.get_param("~base_frame", "base_link")
        self._nominal_topic = rospy.get_param(
            "~nominal_goal_topic", "/factory_search_adapter/nominal_goal"
        )
        self._static_map_topic = rospy.get_param("~static_map_topic", "/map")
        self._local_costmap_topic = rospy.get_param(
            "~local_costmap_topic", "/move_base/local_costmap/costmap"
        )
        self._alignment_status_topic = rospy.get_param(
            "~alignment_status_topic", "/factory_alignment_demo/status"
        )
        self._plan_service_name = rospy.get_param("~make_plan_service", "/move_base/make_plan")
        self._ring_radii = [float(value) for value in rospy.get_param("~ring_radii", [0.4, 0.6])]
        self._ring_directions = int(rospy.get_param("~ring_directions", 8))
        self._footprint_length = float(rospy.get_param("~checked_footprint_length", 0.395))
        self._footprint_width = float(rospy.get_param("~checked_footprint_width", 0.316))
        self._static_occupied_threshold = int(rospy.get_param("~static_occupied_threshold", 65))
        self._local_cost_threshold = int(rospy.get_param("~local_cost_threshold", 80))
        self._local_costmap_timeout = float(rospy.get_param("~local_costmap_timeout_seconds", 2.0))
        self._goal_timeout = float(rospy.get_param("~candidate_goal_timeout_seconds", 45.0))
        self._search_timeout = float(rospy.get_param("~candidate_search_timeout_seconds", 80.0))
        self._server_timeout = float(rospy.get_param("~move_base_server_timeout_seconds", 2.0))
        self._plan_response_timeout = float(rospy.get_param("~make_plan_response_timeout_seconds", 0.4))
        self._timer_period = float(rospy.get_param("~timer_period_seconds", 0.2))
        self._validate_config()

        self._static_map = None
        self._local_costmap = None
        self._local_costmap_monotonic = 0.0
        self._nominal_pose = None
        self._candidates = []
        self._candidate_index = 0
        self._state = "IDLE"
        self._active_sequence = 0
        self._active_deadline = None
        self._search_deadline = None
        self._plan_timeout_this_round = False
        self._plan_fallback_announced = False
        self._last_status = None
        self._completion_locked = False

        self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self._plan_client = rospy.ServiceProxy(self._plan_service_name, GetPlan)
        self._plan_request_queue = queue.Queue(maxsize=1)
        self._plan_worker_thread = threading.Thread(
            target=self._plan_worker, name="factory_make_plan", daemon=True
        )
        self._plan_worker_thread.start()
        self._tf_buffer = tf2_ros.Buffer()
        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
        self._status_pub = rospy.Publisher(
            "/factory_search_adapter/status", String, queue_size=10, latch=True
        )
        self._candidate_pub = rospy.Publisher(
            "/factory_search_adapter/candidates", PoseArray, queue_size=1, latch=True
        )
        self._selected_goal_pub = rospy.Publisher(
            "/factory_search_adapter/selected_goal", PoseStamped, queue_size=1, latch=True
        )
        self._reset_service = rospy.Service("/factory_search_adapter/reset", Trigger, self._reset_callback)
        rospy.Subscriber(self._nominal_topic, PoseStamped, self._nominal_callback, queue_size=1)
        rospy.Subscriber(self._static_map_topic, OccupancyGrid, self._static_map_callback, queue_size=1)
        rospy.Subscriber(self._local_costmap_topic, OccupancyGrid, self._local_costmap_callback, queue_size=1)
        rospy.Subscriber(self._alignment_status_topic, String, self._alignment_status_callback, queue_size=10)
        rospy.Timer(rospy.Duration(self._timer_period), self._timer_callback)
        rospy.on_shutdown(self._cancel_own_goal)
        self._publish_status("WAITING_FOR_NOMINAL_GOAL")

    def _validate_config(self):
        if self._frame_id != "map":
            raise ValueError("factory search candidates must use the map frame")
        if self._ring_directions < 4:
            raise ValueError("ring_directions must be at least 4")
        if any(radius <= 0.0 for radius in self._ring_radii):
            raise ValueError("ring radii must be positive")
        if self._footprint_length <= 0.0 or self._footprint_width <= 0.0:
            raise ValueError("checked footprint dimensions must be positive")
        if not 0 <= self._static_occupied_threshold <= 100:
            raise ValueError("static_occupied_threshold must be in [0, 100]")
        if not 1 <= self._local_cost_threshold <= 100:
            raise ValueError("local_cost_threshold must be in [1, 100]")
        if min(self._local_costmap_timeout, self._goal_timeout, self._search_timeout,
               self._server_timeout, self._plan_response_timeout,
               self._timer_period) <= 0.0:
            raise ValueError("search adapter timeouts must be positive")

    @staticmethod
    def _yaw_from_quaternion(orientation):
        return math.atan2(
            2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
            1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
        )

    @staticmethod
    def _quaternion_from_yaw(yaw):
        return Quaternion(z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0))

    def _publish_status(self, status):
        if status == self._last_status:
            return
        self._last_status = status
        rospy.loginfo("factory search adapter: %s", status)
        self._status_pub.publish(String(data=status))

    def _static_map_callback(self, message):
        if message.header.frame_id.lstrip("/") != self._frame_id:
            rospy.logwarn_throttle(5.0, "factory search ignored static map frame %s", message.header.frame_id)
            return
        self._static_map = message

    def _local_costmap_callback(self, message):
        if message.header.frame_id.lstrip("/") != self._frame_id:
            rospy.logwarn_throttle(
                5.0, "factory search ignored local costmap frame %s (expected %s)",
                message.header.frame_id, self._frame_id,
            )
            return
        self._local_costmap = message
        self._local_costmap_monotonic = time.monotonic()

    def _reset_callback(self, _request):
        self._cancel_own_goal()
        self._active_sequence += 1
        self._nominal_pose = None
        self._candidates = []
        self._candidate_index = 0
        self._active_deadline = None
        self._search_deadline = None
        self._plan_timeout_this_round = False
        self._plan_fallback_announced = False
        self._completion_locked = False
        self._state = "IDLE"
        self._publish_status("RESET_READY_FOR_NEXT_OBSERVATION")
        return TriggerResponse(success=True, message="factory search adapter reset")

    def _nominal_callback(self, message):
        if self._completion_locked:
            self._publish_status("NOMINAL_GOAL_IGNORED_COMPLETE_LOCKED")
            return
        if message.header.frame_id.lstrip("/") != self._frame_id:
            self._publish_status("NOMINAL_GOAL_WRONG_FRAME")
            return
        yaw = self._yaw_from_quaternion(message.pose.orientation)
        values = (message.pose.position.x, message.pose.position.y, yaw)
        if not all(math.isfinite(value) for value in values):
            self._publish_status("NOMINAL_GOAL_INVALID")
            return
        self._cancel_own_goal()
        self._active_sequence += 1
        self._nominal_pose = values
        self._candidates = []
        self._candidate_index = 0
        self._active_deadline = None
        self._search_deadline = None
        self._plan_timeout_this_round = False
        self._plan_fallback_announced = False
        self._state = "PREPARING"
        self._publish_status("PREPARING_CANDIDATES x=%.3f y=%.3f yaw=%.3f" % values)

    def _cancel_own_goal(self):
        if hasattr(self, "_client") and self._state == "NAVIGATING":
            self._client.cancel_goal()

    @staticmethod
    def _grid_origin_yaw(grid):
        return FactorySearchPointAdapter._yaw_from_quaternion(grid.info.origin.orientation)

    def _map_to_grid(self, grid, x, y):
        resolution = grid.info.resolution
        if resolution <= 0.0:
            return None
        origin = grid.info.origin.position
        yaw = self._grid_origin_yaw(grid)
        dx, dy = x - origin.x, y - origin.y
        local_x = math.cos(yaw) * dx + math.sin(yaw) * dy
        local_y = -math.sin(yaw) * dx + math.cos(yaw) * dy
        column, row = int(math.floor(local_x / resolution)), int(math.floor(local_y / resolution))
        if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
            return None
        return row, column

    def _grid_to_map(self, grid, row, column):
        resolution = grid.info.resolution
        origin = grid.info.origin.position
        yaw = self._grid_origin_yaw(grid)
        local_x, local_y = (column + 0.5) * resolution, (row + 0.5) * resolution
        return (
            origin.x + math.cos(yaw) * local_x - math.sin(yaw) * local_y,
            origin.y + math.sin(yaw) * local_x + math.cos(yaw) * local_y,
        )

    @staticmethod
    def _cost_at(grid, row, column):
        return grid.data[row * grid.info.width + column]

    def _footprint_cost(self, grid, x, y, yaw, threshold):
        """Return max cell cost, or None if footprint reaches unknown/outside/lethal cells."""
        resolution = grid.info.resolution
        if resolution <= 0.0:
            return None
        radius = math.hypot(self._footprint_length / 2.0, self._footprint_width / 2.0)
        centre = self._map_to_grid(grid, x, y)
        if centre is None:
            return None
        radius_cells = int(math.ceil(radius / resolution)) + 1
        max_cost = 0
        found = False
        for row in range(centre[0] - radius_cells, centre[0] + radius_cells + 1):
            for column in range(centre[1] - radius_cells, centre[1] + radius_cells + 1):
                if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
                    return None
                cell_x, cell_y = self._grid_to_map(grid, row, column)
                dx, dy = cell_x - x, cell_y - y
                longitudinal = math.cos(yaw) * dx + math.sin(yaw) * dy
                lateral = -math.sin(yaw) * dx + math.cos(yaw) * dy
                if (abs(longitudinal) > self._footprint_length / 2.0
                        or abs(lateral) > self._footprint_width / 2.0):
                    continue
                found = True
                cost = self._cost_at(grid, row, column)
                if cost < 0 or cost >= threshold:
                    return None
                max_cost = max(max_cost, cost)
        return max_cost if found else None

    def _footprint_is_inside_grid(self, grid, x, y, yaw):
        """Whether the complete checked rectangle is covered by this grid.

        The local costmap is a rolling window.  A candidate farther than that
        window is not evidence of an obstacle; it simply cannot yet be checked
        against live cone observations.  Static-map and global-plan checks
        still apply in that case, and TEB will receive the current local map
        while driving there.
        """
        half_length = self._footprint_length / 2.0
        half_width = self._footprint_width / 2.0
        for longitudinal in (-half_length, half_length):
            for lateral in (-half_width, half_width):
                corner_x = x + math.cos(yaw) * longitudinal - math.sin(yaw) * lateral
                corner_y = y + math.sin(yaw) * longitudinal + math.cos(yaw) * lateral
                if self._map_to_grid(grid, corner_x, corner_y) is None:
                    return False
        return True

    def _fresh_local_costmap(self):
        return (self._local_costmap is not None
                and time.monotonic() - self._local_costmap_monotonic <= self._local_costmap_timeout)

    def _current_pose(self):
        try:
            transform = self._tf_buffer.lookup_transform(
                self._frame_id, self._base_frame, rospy.Time(0), rospy.Duration(0.2)
            )
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException, tf2_ros.TimeoutException):
            return None
        translation = transform.transform.translation
        return translation.x, translation.y, self._yaw_from_quaternion(transform.transform.rotation)

    def _pose_stamped(self, x, y, yaw):
        pose = PoseStamped()
        pose.header.frame_id = self._frame_id
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation = self._quaternion_from_yaw(yaw)
        return pose

    def _plan_worker(self):
        """Serialize potentially stuck service calls in one daemon worker."""
        while True:
            task = self._plan_request_queue.get()
            try:
                task.response = self._plan_client(task.request)
            except Exception as error:  # rospy may expose several service exceptions.
                task.error = error
            finally:
                task.finished.set()

    def _announce_plan_fallback(self):
        if self._plan_fallback_announced:
            return
        self._plan_fallback_announced = True
        self._publish_status("MAKE_PLAN_TIMEOUT_FALLBACK_TO_MOVE_BASE")

    def _plan_length(self, x, y, yaw):
        """Return reachable, length, and whether a live plan was obtained.

        The timer thread never calls the service directly.  A stalled RPC can
        leave one daemon worker blocked, but this search round immediately
        falls back and no additional make_plan calls are queued for its other
        candidates.
        """
        if self._plan_timeout_this_round:
            return True, float("inf"), False
        # Do not probe the service from the timer callback.  In the field a
        # registered service can still stall during a transport handshake.
        # The only RPC is therefore made by _plan_worker below.
        start = self._current_pose()
        if start is None:
            self._plan_timeout_this_round = True
            self._announce_plan_fallback()
            return True, float("inf"), False
        request = GetPlanRequest()
        request.start = self._pose_stamped(*start)
        request.goal = self._pose_stamped(x, y, yaw)
        request.tolerance = 0.0
        task = _PlanTask(request)
        try:
            self._plan_request_queue.put_nowait(task)
        except queue.Full:
            self._plan_timeout_this_round = True
            self._announce_plan_fallback()
            return True, float("inf"), False
        if not task.finished.wait(self._plan_response_timeout):
            self._plan_timeout_this_round = True
            self._announce_plan_fallback()
            return True, float("inf"), False
        if task.error is not None or task.response is None:
            rospy.logwarn_throttle(2.0, "factory search make_plan failed: %s", task.error)
            self._plan_timeout_this_round = True
            self._announce_plan_fallback()
            return True, float("inf"), False
        poses = task.response.plan.poses
        if len(poses) < 2:
            return False, None, True
        length = 0.0
        for first, second in zip(poses, poses[1:]):
            dx = second.pose.position.x - first.pose.position.x
            dy = second.pose.position.y - first.pose.position.y
            length += math.hypot(dx, dy)
        return True, length, True

    def _candidate_valid(self, candidate, with_plan):
        if self._static_map is None:
            return False, "STATIC_MAP_UNAVAILABLE", None
        static_cost = self._footprint_cost(
            self._static_map, candidate["x"], candidate["y"], candidate["yaw"],
            self._static_occupied_threshold,
        )
        if static_cost is None:
            return False, "STATIC_FOOTPRINT_BLOCKED", None
        local_costmap_checked = (
            self._fresh_local_costmap()
            and self._footprint_is_inside_grid(
                self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"])
        )
        if local_costmap_checked:
            local_cost = self._footprint_cost(
                self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"],
                self._local_cost_threshold,
            )
            if local_cost is None:
                return False, "LOCAL_FOOTPRINT_BLOCKED", None
        else:
            # A rolling local costmap may be absent or not cover a distant
            # candidate.  That is not a cone collision: retain the static-map
            # and global-plan checks, then let TEB use live obstacles in motion.
            local_cost = self._local_cost_threshold - 1
        plan_length = candidate.get("plan_length", float("inf"))
        make_plan_checked = candidate.get("make_plan_checked", False)
        if with_plan:
            reachable, plan_length, make_plan_checked = self._plan_length(
                candidate["x"], candidate["y"], candidate["yaw"]
            )
            if reachable is None:
                return False, "MAP_BASE_TF_UNAVAILABLE", None
            if not reachable:
                return False, "GLOBAL_PLAN_UNREACHABLE", None
        candidate["local_cost"] = local_cost
        candidate["local_costmap_checked"] = local_costmap_checked
        candidate["make_plan_checked"] = make_plan_checked
        candidate["plan_length"] = plan_length
        return True, None, candidate

    def _generate_raw_candidates(self):
        x, y, yaw = self._nominal_pose
        generated = [{"x": x, "y": y, "yaw": yaw, "radius": 0.0}]
        seen = {(round(x, 4), round(y, 4))}
        for radius in self._ring_radii:
            for index in range(self._ring_directions):
                angle = 2.0 * math.pi * index / self._ring_directions
                cx, cy = x + radius * math.cos(angle), y + radius * math.sin(angle)
                key = round(cx, 4), round(cy, 4)
                if key not in seen:
                    seen.add(key)
                    generated.append({"x": cx, "y": cy, "yaw": yaw, "radius": radius})
        return generated

    def _publish_candidates(self):
        message = PoseArray()
        message.header.frame_id = self._frame_id
        message.header.stamp = rospy.Time.now()
        for candidate in self._candidates:
            pose = Pose()
            pose.position.x, pose.position.y = candidate["x"], candidate["y"]
            pose.orientation = self._quaternion_from_yaw(candidate["yaw"])
            message.poses.append(pose)
        self._candidate_pub.publish(message)

    def _prepare_candidates(self):
        if self._static_map is None:
            self._publish_status("WAITING_FOR_STATIC_MAP")
            return
        if not self._fresh_local_costmap():
            self._publish_status("LOCAL_COSTMAP_UNAVAILABLE_STATIC_PLAN_FALLBACK")
        accepted = []
        for candidate in self._generate_raw_candidates():
            valid, _reason, value = self._candidate_valid(candidate, with_plan=True)
            if valid:
                accepted.append(value)
        if not accepted:
            self._state = "FAILED"
            self._publish_status("NO_SAFE_SEARCH_CANDIDATES")
            return
        accepted.sort(key=lambda item: (item["radius"], item["local_cost"], item["plan_length"]))
        self._candidates = accepted
        self._candidate_index = 0
        self._publish_candidates()
        self._publish_status("CANDIDATES_READY count=%d" % len(self._candidates))
        self._send_next_candidate("INITIAL")

    def _send_next_candidate(self, reason):
        while self._candidate_index < len(self._candidates):
            candidate = self._candidates[self._candidate_index]
            self._candidate_index += 1
            valid, invalid_reason, candidate = self._candidate_valid(candidate, with_plan=True)
            if not valid:
                self._publish_status("CANDIDATE_SKIPPED reason=%s" % invalid_reason)
                continue
            if not self._client.wait_for_server(rospy.Duration(self._server_timeout)):
                self._state = "FAILED"
                self._publish_status("MOVE_BASE_UNAVAILABLE")
                return
            goal = MoveBaseGoal()
            goal.target_pose = self._pose_stamped(candidate["x"], candidate["y"], candidate["yaw"])
            self._active_sequence += 1
            sequence = self._active_sequence
            self._state = "NAVIGATING"
            self._active_deadline = time.monotonic() + self._goal_timeout
            self._selected_goal_pub.publish(goal.target_pose)
            self._client.send_goal(
                goal,
                done_cb=lambda state, result, seq=sequence, item=candidate: self._goal_done(seq, item, state, result),
            )
            self._publish_status(
                "CANDIDATE_SENT %d/%d reason=%s local_costmap=%s make_plan=%s x=%.3f y=%.3f yaw=%.3f" % (
                    self._candidate_index, len(self._candidates), reason,
                    "CHECKED" if candidate["local_costmap_checked"] else "FALLBACK",
                    "CHECKED" if candidate["make_plan_checked"] else "FALLBACK",
                    candidate["x"], candidate["y"], candidate["yaw"],
                )
            )
            return
        self._state = "FAILED"
        self._active_deadline = None
        self._publish_status("ALL_SEARCH_CANDIDATES_EXHAUSTED")

    def _goal_done(self, sequence, candidate, state, _result):
        if sequence != self._active_sequence or self._state != "NAVIGATING":
            return
        self._active_deadline = None
        if state == GoalStatus.SUCCEEDED:
            self._state = "WAITING_FOR_SEARCH"
            self._search_deadline = time.monotonic() + self._search_timeout
            self._publish_status(
                "CANDIDATE_REACHED %d/%d; WAITING_FOR_FACTORY_SEARCH" % (
                    self._candidate_index, len(self._candidates)
                )
            )
            return
        if state in self._NAV_FAILURE_STATES:
            self._publish_status("CANDIDATE_NAV_FAILED state=%d; TRYING_NEXT" % state)
        else:
            self._publish_status("CANDIDATE_NAV_UNEXPECTED state=%d; TRYING_NEXT" % state)
        self._send_next_candidate("NAVIGATION_FAILURE")

    def _alignment_status_callback(self, message):
        if self._state != "WAITING_FOR_SEARCH":
            return
        status = message.data
        if status.startswith("FACTORY_ENTRY_COMPLETE"):
            self._state = "COMPLETE"
            self._completion_locked = True
            self._search_deadline = None
            self._publish_status("FACTORY_ENTRY_COMPLETE; REMAINING_CANDIDATES_CANCELLED_LOCKED")
            return
        if status.startswith("FACTORY_MISMATCH"):
            self._state = "FAILED"
            self._search_deadline = None
            self._publish_status("OBSERVATION_MISMATCH")
            return
        entry_goal_failed = (
            status.startswith("ENTRY_GOAL_")
            and not status.startswith("ENTRY_GOAL_SENT")
        )
        if status.startswith(self._SEARCH_RETRY_PREFIXES) or entry_goal_failed:
            self._search_deadline = None
            self._publish_status("FACTORY_SEARCH_FAILED status=%s; TRYING_NEXT" % status)
            self._send_next_candidate("SEARCH_FAILURE")

    def _timer_callback(self, _event):
        try:
            self._timer_tick()
        except Exception as error:
            rospy.logerr("factory search timer error: %s", error)
            self._publish_status("ADAPTER_TIMER_ERROR")

    def _timer_tick(self):
        if self._state == "PREPARING":
            self._prepare_candidates()
            return
        active_deadline = self._active_deadline
        if (self._state == "NAVIGATING"
                and active_deadline is not None
                and time.monotonic() > active_deadline):
            self._client.cancel_goal()
            self._active_sequence += 1
            self._active_deadline = None
            self._publish_status("CANDIDATE_NAV_TIMEOUT; TRYING_NEXT")
            self._send_next_candidate("NAVIGATION_TIMEOUT")
            return
        search_deadline = self._search_deadline
        if (self._state == "WAITING_FOR_SEARCH"
                and search_deadline is not None
                and time.monotonic() > search_deadline):
            self._search_deadline = None
            self._publish_status("FACTORY_SEARCH_TIMEOUT; TRYING_NEXT")
            self._send_next_candidate("SEARCH_TIMEOUT")


def main():
    try:
        FactorySearchPointAdapter()
        rospy.spin()
    except (ValueError, rospy.ROSException) as error:
        rospy.logfatal("factory search adapter did not start: %s", error)
        raise


if __name__ == "__main__":
    main()