Ejemplo n.º 1
0
def main():
    # INIT RCL ================================================================
    rclpy.init()
    adpt.init_rclcpp()

    # INIT GRAPH ==============================================================
    map_name = "test_map"
    test_graph = graph.Graph()

    test_graph.add_waypoint(map_name, [0.0, -10.0])  # 0
    test_graph.add_waypoint(map_name, [0.0, -5.0])  # 1
    test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True)  # 2
    test_graph.add_waypoint(map_name, [-10.0, 0])  # 3
    test_graph.add_waypoint(map_name, [-5.0, 0.0])  # 4
    test_graph.add_waypoint(map_name, [0.0, 0.0])  # 5
    test_graph.add_waypoint(map_name, [5.0, 0.0])  # 6
    test_graph.add_waypoint(map_name, [10.0, 0.0])  # 7
    test_graph.add_waypoint(map_name, [0.0, 5.0])  # 8
    test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True)  # 9
    test_graph.add_waypoint(map_name, [0.0, 10.0])  # 10

    assert test_graph.get_waypoint(2).holding_point
    assert test_graph.get_waypoint(9).holding_point
    assert not test_graph.get_waypoint(10).holding_point
    """
                     10(D)
                      |
                      |
                      8------9
                      |      |
                      |      |
        3------4------5------6------7(D)
                      |      |
                      |      |
                      1------2
                      |
                      |
                      0
   """

    test_graph.add_bidir_lane(0, 1)  # 0   1
    test_graph.add_bidir_lane(1, 2)  # 2   3
    test_graph.add_bidir_lane(1, 5)  # 4   5
    test_graph.add_bidir_lane(2, 6)  # 6   7
    test_graph.add_bidir_lane(3, 4)  # 8   9
    test_graph.add_bidir_lane(4, 5)  # 10 11
    test_graph.add_bidir_lane(5, 6)  # 12 13
    test_graph.add_dock_lane(6, 7, "A")  # 14 15
    test_graph.add_bidir_lane(5, 8)  # 16 17
    test_graph.add_bidir_lane(6, 9)  # 18 19
    test_graph.add_bidir_lane(8, 9)  # 20 21
    test_graph.add_dock_lane(8, 10, "B")  # 22 23

    assert test_graph.num_lanes == 24

    test_graph.add_key(pickup_name, 7)
    test_graph.add_key(dropoff_name, 10)

    assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
        and dropoff_name in test_graph.keys

    # INIT FLEET ==============================================================
    profile = traits.Profile(geometry.make_final_convex_circle(1.0))
    robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
                                        angular=traits.Limits(1.0, 0.45),
                                        profile=profile)

    # Manages delivery or loop requests
    adapter = adpt.MockAdapter("TestDeliveryAdapter")
    fleet = adapter.add_fleet("test_fleet", robot_traits, test_graph)
    fleet.accept_delivery_requests(lambda x: True)

    cmd_node = Node("RobotCommandHandle")

    # Test compute_plan_starts, which tries to place the robot on the navgraph
    # Your robot MUST be near a waypoint or lane for this to work though!
    starts = plan.compute_plan_starts(test_graph, "test_map",
                                      [[-10.0], [0.0], [0.0]], adapter.now())
    assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]

    # Alternatively, if you DO know where your robot is, place it directly!
    starts = [plan.Start(adapter.now(), 0, 0.0)]

    # Lambda to insert an adapter
    def updater_inserter(handle_obj, updater):
        handle_obj.updater = updater

    # Manages and executes robot commands
    robot_cmd = MockRobotCommand(cmd_node, test_graph)

    fleet.add_robot(robot_cmd, "T0", profile, starts,
                    partial(updater_inserter, robot_cmd))

    # INIT TASK OBSERVER ======================================================
    at_least_one_incomplete = False
    completed = False

    def task_cb(msg):
        nonlocal at_least_one_incomplete
        nonlocal completed

        if msg.state == TASK_STATE_COMPLETED:
            completed = True
            at_least_one_incomplete = False
        else:
            completed = False
            at_least_one_incomplete = True

    task_node = rclpy.create_node("task_summary_node")
    task_node.create_subscription(TaskSummary, "task_summaries", task_cb, 10)

    # INIT DISPENSERS =========================================================
    quiet_dispenser = MockQuietDispenser(quiet_dispenser_name)
    flaky_dispenser = MockFlakyDispenser(flaky_dispenser_name)

    # FINAL PREP ==============================================================
    rclpy_executor = SingleThreadedExecutor()
    rclpy_executor.add_node(task_node)
    rclpy_executor.add_node(cmd_node)
    rclpy_executor.add_node(quiet_dispenser)
    rclpy_executor.add_node(flaky_dispenser)

    last_quiet_state = False
    last_flaky_state = False

    # GO! =====================================================================
    adapter.start()

    print("# SENDING NEW REQUEST ############################################")
    request = adpt.type.CPPDeliveryMsg("test_delivery", pickup_name,
                                       quiet_dispenser_name, dropoff_name,
                                       flaky_dispenser_name)
    quiet_dispenser.reset()
    flaky_dispenser.reset()
    adapter.request_delivery(request)
    rclpy_executor.spin_once(1)

    for i in range(1000):
        if quiet_dispenser.success_flag != last_quiet_state:
            last_quiet_state = quiet_dispenser.success_flag
            print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
                  last_quiet_state)

        if flaky_dispenser.success_flag != last_flaky_state:
            last_flaky_state = flaky_dispenser.success_flag
            print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
                  last_flaky_state)

        if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
            rclpy_executor.spin_once(1)
            break

        rclpy_executor.spin_once(1)
        time.sleep(0.2)

    print("\n== DEBUG TASK REPORT ==")
    print("Visited waypoints:", robot_cmd.visited_waypoints)
    print("At least one incomplete:", at_least_one_incomplete)
    print("Completed:", completed)
    print()

    assert len(robot_cmd.visited_waypoints) == 6
    assert all([x in robot_cmd.visited_waypoints for x in [0, 5, 6, 7, 8, 10]])
    assert robot_cmd.visited_waypoints[0] == 2
    assert robot_cmd.visited_waypoints[5] == 4
    assert robot_cmd.visited_waypoints[6] == 3
    assert robot_cmd.visited_waypoints[7] == 1
    assert robot_cmd.visited_waypoints[8] == 2
    assert robot_cmd.visited_waypoints[10] == 1
    assert at_least_one_incomplete

    # Uncomment this to send a second request.

    # TODO(CH3):
    # But note! The TaskManager has to be fixed first to allow task pre-emption
    # print("# SENDING NEW REQUEST ##########################################")
    # request = adpt.type.CPPDeliveryMsg("test_delivery_two",
    #                                    dropoff_name,
    #                                    flaky_dispenser_name,
    #                                    pickup_name,
    #                                    quiet_dispenser_name)
    # quiet_dispenser.reset()
    # flaky_dispenser.reset()
    # adapter.request_delivery(request)
    # rclpy_executor.spin_once(1)
    #
    # for i in range(1000):
    #     if quiet_dispenser.success_flag != last_quiet_state:
    #         last_quiet_state = quiet_dispenser.success_flag
    #         print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==",
    #               last_quiet_state)
    #
    #     if flaky_dispenser.success_flag != last_flaky_state:
    #         last_flaky_state = flaky_dispenser.success_flag
    #         print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==",
    #               last_flaky_state)
    #
    #     if quiet_dispenser.success_flag and flaky_dispenser.success_flag:
    #         rclpy_executor.spin_once(1)
    #         break
    #
    #     rclpy_executor.spin_once(1)
    #     time.sleep(0.2)
    #
    # print("\n== DEBUG TASK REPORT ==")
    # print("Visited waypoints:", robot_cmd.visited_waypoints)
    # print("At least one incomplete:", at_least_one_incomplete)
    # print("Completed:", completed)
    # print()

    task_node.destroy_node()
    cmd_node.destroy_node()
    quiet_dispenser.destroy_node()
    flaky_dispenser.destroy_node()

    rclpy_executor.shutdown()
    rclpy.shutdown()
Ejemplo n.º 2
0
def main():
    # INIT RCL ================================================================
    rclpy.init()

    try:
        adpt.init_rclcpp()
    except RuntimeError:
        # Continue if it is already initialized
        pass

    # INIT GRAPH ==============================================================
    test_graph = graph.Graph()

    test_graph.add_waypoint(map_name, [0.0, -10.0])  # 0
    test_graph.add_waypoint(map_name, [0.0, -5.0])  # 1
    test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True)  # 2
    test_graph.add_waypoint(map_name, [-10.0, 0])  # 3
    test_graph.add_waypoint(map_name, [-5.0, 0.0])  # 4
    test_graph.add_waypoint(map_name, [0.0, 0.0])  # 5
    test_graph.add_waypoint(map_name, [5.0, 0.0])  # 6
    test_graph.add_waypoint(map_name, [10.0, 0.0])  # 7
    test_graph.add_waypoint(map_name, [0.0, 5.0])  # 8
    test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True)  # 9
    test_graph.add_waypoint(map_name, [0.0, 10.0])  # 10

    assert test_graph.get_waypoint(2).holding_point
    assert test_graph.get_waypoint(9).holding_point
    assert not test_graph.get_waypoint(10).holding_point

    test_graph_legend = \
        """
        D : Dispenser
        I : Ingestor
        H : Holding Point
        K : Dock
        Numerals : Waypoints
        ---- : Lanes
        """

    test_graph_vis = \
        test_graph_legend + \
        """
                         10(I,K)
                          |
                          |
                          8------9(H)
                          |      |
                          |      |
            3------4------5------6------7(D,K)
                          |      |
                          |      |
                          1------2(H)
                          |
                          |
                          0
       """

    test_graph.add_bidir_lane(0, 1)  # 0   1
    test_graph.add_bidir_lane(1, 2)  # 2   3
    test_graph.add_bidir_lane(1, 5)  # 4   5
    test_graph.add_bidir_lane(2, 6)  # 6   7
    test_graph.add_bidir_lane(3, 4)  # 8   9
    test_graph.add_bidir_lane(4, 5)  # 10 11
    test_graph.add_bidir_lane(5, 6)  # 12 13
    test_graph.add_dock_lane(6, 7, "A")  # 14 15
    test_graph.add_bidir_lane(5, 8)  # 16 17
    test_graph.add_bidir_lane(6, 9)  # 18 19
    test_graph.add_bidir_lane(8, 9)  # 20 21
    test_graph.add_dock_lane(8, 10, "B")  # 22 23

    assert test_graph.num_lanes == 24

    test_graph.add_key(pickup_name, 7)
    test_graph.add_key(dropoff_name, 10)

    assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \
        and dropoff_name in test_graph.keys

    # INIT FLEET ==============================================================
    profile = traits.Profile(geometry.make_final_convex_circle(1.0))
    robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3),
                                        angular=traits.Limits(1.0, 0.45),
                                        profile=profile)

    # Manages delivery or loop requests
    adapter = adpt.MockAdapter("TestDeliveryAdapter")
    fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph)

    # Set up task request callback function
    # we will only accept delivery task here
    def task_request_cb(task_profile):
        from rmf_task_msgs.msg import TaskType
        if (task_profile.description.task_type == TaskType.TYPE_DELIVERY):
            return True
        else:
            return False

    fleet.accept_task_requests(task_request_cb)

    # Set fleet battery profile
    battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8)
    mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22)
    motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
    ambient_power_sys = battery.PowerSystem.make(20.0)
    ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
                                                 ambient_power_sys)
    tool_power_sys = battery.PowerSystem.make(10.0)
    tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)

    fleet.set_recharge_threshold(0.2)
    b_success = fleet.set_task_planner_params(battery_sys, motion_sink,
                                              ambient_sink, tool_sink)

    assert b_success, "set battery param failed"

    cmd_node = Node("RobotCommandHandle")

    # Test compute_plan_starts, which tries to place the robot on the navgraph
    # Your robot MUST be near a waypoint or lane for this to work though!
    starts = plan.compute_plan_starts(test_graph, map_name,
                                      [[-10.0], [0.0], [0.0]], adapter.now())
    assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts]

    # Alternatively, if you DO know where your robot is, place it directly!
    starts = [plan.Start(adapter.now(), 0, 0.0)]

    # Lambda to insert an adapter
    def updater_inserter(handle_obj, updater):
        updater.update_battery_soc(1.0)
        handle_obj.updater = updater

    # Manages and executes robot commands
    robot_cmd = MockRobotCommand(cmd_node, test_graph)

    fleet.add_robot(robot_cmd, "T0", profile, starts,
                    partial(updater_inserter, robot_cmd))

    # INIT DISPENSERS =========================================================
    dispenser = MockDispenser(dispenser_name)
    ingestor = MockIngestor(ingestor_name)

    # INIT TASK SUMMARY OBSERVER ==============================================
    # Note: this is used for assertation check on TaskSummary.Msg
    observer = TaskSummaryObserver()

    # FINAL PREP ==============================================================
    rclpy_executor = SingleThreadedExecutor()
    rclpy_executor.add_node(cmd_node)
    rclpy_executor.add_node(dispenser)
    rclpy_executor.add_node(ingestor)
    rclpy_executor.add_node(observer)

    # GO! =====================================================================
    adapter.start()

    print("\n")
    print("# SENDING SINGLE DELIVERY REQUEST ################################")
    print(test_graph_vis)

    dispenser.reset()
    ingestor.reset()
    observer.reset()
    observer.add_task(test_name)

    task_desc = Type.CPPTaskDescriptionMsg()
    # this is the time when the robot reaches the pick up point
    task_desc.start_time_sec = int(time.time()) + 50
    task_desc.delivery = Type.CPPDeliveryMsg(pickup_name, dispenser_name,
                                             dropoff_name, ingestor_name)
    task_profile = Type.CPPTaskProfileMsg()
    task_profile.description = task_desc
    task_profile.task_id = test_name
    adapter.dispatch_task(task_profile)

    rclpy_executor.spin_once(1)

    for i in range(1000):
        if observer.all_tasks_complete():
            print("Tasks Complete.")
            break
        rclpy_executor.spin_once(1)
        # time.sleep(0.2)

    results = observer.count_successful_tasks()
    print("\n== DEBUG TASK REPORT ==")
    print("Visited waypoints:", robot_cmd.visited_waypoints)
    print(f"Sucessful Tasks: {results[0]} / {results[1]}")

    assert results[0] == results[1], "Not all tasks were completed."

    error_msg = "Robot did not take the expected route"
    assert robot_cmd.visited_waypoints == [
        0, 0, 5, 5, 6, 6, 7, 6, 5, 5, 8, 8, 10
    ], error_msg

    # check if unstable partcipant works
    # this is helpful for to update the robot when it is
    print("Update a custom itinerary to fleet adapter")
    traj = schedule.make_trajectory(robot_traits, adapter.now(),
                                    [[3, 0, 0], [1, 1, 0], [2, 1, 0]])
    route = schedule.Route("test_map", traj)

    participant = robot_cmd.updater.get_unstable_participant()
    routeid = participant.last_route_id
    participant.set_itinerary([route])
    new_routeid = participant.last_route_id
    print(f"Previous route id: {routeid} , new route id: {new_routeid}")
    assert routeid != new_routeid

    # TODO(YL) There's an issue with during shutdown of the adapter, occurs
    # when set_itinerary() function above is used. Similarly with a non-mock
    # adpater, will need to address this in near future
    print("\n~ Shutting Down everything ~")

    cmd_node.destroy_node()
    observer.destroy_node()
    dispenser.destroy_node()
    ingestor.destroy_node()

    robot_cmd.stop()
    adapter.stop()
    rclpy_executor.shutdown()
    rclpy.shutdown()
Ejemplo n.º 3
0
def create_robot_command_handles(config, handle_data, dry_run=False):
    robots = {}

    for robot_name, robot_config in config['robots'].items():
        # CONFIGURE MIR =======================================================
        mir_config = robot_config['mir_config']
        rmf_config = robot_config['rmf_config']

        configuration = mir100_client.Configuration()
        configuration.username = mir_config['base_url']
        configuration.host = mir_config['user']
        configuration.password = mir_config['password']

        api_client = mir100_client.ApiClient(configuration)
        api_client.default_headers['Accept-Language'] = 'en-US'

        # CONFIGURE HANDLE ====================================================
        robot = MiRCommandHandle(name=robot_name,
                                 node=handle_data['node'],
                                 rmf_graph=handle_data['graph'],
                                 robot_state_update_frequency=rmf_config.get(
                                     'robot_state_update_frequency', 1),
                                 dry_run=dry_run)
        robot.mir_api = mir100_client.DefaultApi(api_client)
        robot.transforms = handle_data['transforms']
        robot.rmf_map_name = rmf_config['start']['map_name']

        if not dry_run:
            with MiRRetryContext(robot):
                _mir_status = robot.mir_api.status_get()
                robot.mir_name = _mir_status.robot_name

                robot.load_mir_missions()
                robot.load_mir_positions()
        else:
            robot.mir_name = "DUMMY_ROBOT_FOR_DRY_RUN"

        robots[robot.name] = robot

        # OBTAIN PLAN STARTS ==================================================
        start_config = rmf_config['start']

        # If the plan start is configured, use it, otherwise, grab it
        if (start_config.get('waypoint_index')
                and start_config.get('orientation')):
            starts = [
                plan.Start(handle_data['adapter'].now(),
                           start_config.get('waypoint_index'),
                           start_config.get('orientation'))
            ]
        else:
            starts = plan.compute_plan_starts(
                handle_data['graph'], start_config['map_name'],
                robot.get_position(rmf=True, as_dimensions=True),
                handle_data['adapter'].now())

        assert starts, ("Robot %s can't be placed on the nav graph!" %
                        robot_name)

        # Insert start data into robot
        start = starts[0]

        if start.lane.has_value:  # If the robot is in a lane
            robot.rmf_current_lane_index = start.lane.value
            robot.rmf_current_waypoint_index = None
            robot.rmf_target_waypoint_index = None
        else:  # Otherwise, the robot is on a waypoint
            robot.rmf_current_lane_index = None
            robot.rmf_current_waypoint_index = start.waypoint
            robot.rmf_target_waypoint_index = None

        print("MAP_NAME:", start_config['map_name'])
        robot.rmf_map_name = start_config['map_name']

        # INSERT UPDATER ======================================================
        def updater_inserter(handle_obj, rmf_updater):
            """Insert a RobotUpdateHandle."""
            handle_obj.rmf_updater = rmf_updater

        handle_data['fleet_handle'].add_robot(robot, robot.name,
                                              handle_data['profile'], starts,
                                              partial(updater_inserter, robot))

        handle_data['node'].get_logger().info(
            f"successfully initialized robot {robot.name}"
            f" (MiR name: {robot.mir_name})")

    return robots
Ejemplo n.º 4
0
    def __init__(self, name, config, node, graph, vehicle_traits, transforms,
                 map_name, initial_waypoint, initial_orientation,
                 charger_waypoint, update_frequency, adapter):
        adpt.RobotCommandHandle.__init__(self)
        self.name = name
        self.config = config
        self.node = node
        self.graph = graph
        self.vehicle_traits = vehicle_traits
        self.transforms = transforms
        self.map_name = map_name
        self.initial_waypoint = initial_waypoint
        self.initial_orientation = initial_orientation
        # Get the index of the charger waypoint
        waypoint = self.graph.find_waypoint(charger_waypoint)
        assert waypoint, f"Charger waypoint {charger_waypoint} \
          does not exist in the navigation graph"

        self.charger_waypoint_index = waypoint.index
        self.charger_is_set = False
        self.update_frequency = update_frequency
        self.update_handle = None  # RobotUpdateHandle
        self.battery_soc = 1.0
        self.api = None
        self.position = []  # (x,y,theta) in RMF coordinates (meters, radians)
        self.initialized = False
        self.state = RobotState.IDLE
        self.dock_name = ""
        self.adapter = adapter

        self.requested_waypoints = []  # RMF Plan waypoints
        self.remaining_waypoints = []
        self.path_finished_callback = None
        self.next_arrival_estimator = None
        self.path_index = 0
        self.docking_finished_callback = None

        # RMF location trackers
        self.last_known_lane_index = None
        self.last_known_waypoint_index = None
        # if robot is waiting at a waypoint. This is a Graph::Waypoint index
        self.on_waypoint = None
        # if robot is travelling on a lane. This is a Graph::Lane index
        self.on_lane = None
        self.target_waypoint = None  # this is a Plan::Waypoint
        # The graph index of the waypoint the robot is currently docking into
        self.dock_waypoint_index = None

        # Threading variables
        self._lock = threading.Lock()
        self._follow_path_thread = None
        self._quit_path_event = threading.Event()
        self._dock_thread = None
        self._quit_dock_event = threading.Event()

        # Establish connection with the robot
        self.api = RobotAPI(
            self.config['base_url'],
            self.config['user'],
            self.config['password'],
            robot_name=name,
            config=self.config,
            vehicle_traits=vehicle_traits,
        )
        assert self.api.connected, "Unable to connect to Robot API server"

        self.position = self.get_position()  # RMF coordinates
        assert len(
            self.position) > 2, "Unable to get current location of the robot"
        self.node.get_logger().info(
            f"The robot is starting at: [{self.position[0]:.2f}, "
            f"{self.position[1]:.2f}, {self.position[2]:.2f}]")
        # Obtain StartSet for the robot
        self.starts = []
        time_now = self.adapter.now()
        if (self.initial_waypoint is not None) and\
                (self.initial_orientation is not None):
            self.node.get_logger().info(
                f"Using provided initial waypoint [{self.initial_waypoint}] "
                f"and orientation [{self.initial_orientation:.2f}] to "
                f"initialize starts for robot [{self.name}]")
            # Get the waypoint index for initial_waypoint
            initial_waypoint_index = self.graph.find_waypoint(
                self.initial_waypoint).index
            self.starts = [
                plan.Start(time_now, initial_waypoint_index,
                           self.initial_orientation)
            ]
        else:
            self.node.get_logger().info(
                f"Running compute_plan_starts for robot:{self.name}")
            self.starts = plan.compute_plan_starts(self.graph, self.map_name,
                                                   self.position, time_now)

        if self.starts is None or len(self.starts) == 0:
            self.node.get_logger().error(
                f"Unable to determine StartSet for {self.name}")
            return
        start = self.starts[0]

        # Update tracking variables
        if start.lane is not None:  # If the robot is on a lane
            self.last_known_lane_index = start.lane
            self.on_lane = start.lane
            self.last_known_waypoint_index = start.waypoint
        else:  # Otherwise, the robot is on a waypoint
            self.last_known_waypoint_index = start.waypoint
            self.on_waypoint = start.waypoint

        self.state_update_timer = self.node.create_timer(
            1.0 / self.update_frequency, self.update)

        self.initialized = True