Пример #1
0
 def test_error(self):
     dom = [[0, 0], [1, 1], [2, 2]]
     ran = [[0, -1], [1, 2], [2, -1]]
     t = nudged.estimate(dom, ran)
     self.assertEqual(t.transform(dom), [[0, 0], [1, 0], [2, 0]])
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 2.0)
Пример #2
0
 def test_error(self):
     dom = [(0, 0), (1, 1), (2, 2)]
     ran = [(0, -1), (1, 2), (2, -1)]
     t = nudged.estimate(dom, ran)
     self.assertEqual(t.transform(dom), [(0, 0), (1, 0), (2, 0)])
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 2.0)
Пример #3
0
 def test_error(self):
     dom = [[0,0],  [1,1],      [2, 2]]
     ran = [[0,-1], [1,2], [2,-1]]
     t = nudged.estimate(dom, ran)
     self.assertEqual(t.transform(dom), [[0,0], [1,0], [2,0]])
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 2.0)
    def __init__(self, fleet_config):
        super().__init__('fleet_driver_mir')
        self.fleet_config = fleet_config
        self.robots = {}
        self.api_clients = []
        self.status_pub = self.create_publisher(FleetState, 'fleet_states', 1)
        self.pub_timer = self.create_timer(self.STATUS_PUB_PERIOD,
                                           self.pub_fleet)
        self.ref_coordinates_rmf = [[26.95, -20.23], [29.26, -22.38],
                                    [11.4, -16.48], [12.46, -16.99]]
        self.ref_coordinates_mir = [[7.2, 16.6], [5.15, 18.35], [23, 12.35],
                                    [22.05, 12.95]]
        self.rmf2mir_transform = nudged.estimate(self.ref_coordinates_rmf,
                                                 self.ref_coordinates_mir)
        self.mir2rmf_transform = nudged.estimate(self.ref_coordinates_mir,
                                                 self.ref_coordinates_rmf)
        mse = nudged.estimate_error(self.rmf2mir_transform,
                                    self.ref_coordinates_rmf,
                                    self.ref_coordinates_mir)
        self.get_logger().info(f'transformation estimate error: {mse}')

        for api_client in self.create_all_api_clients(self.fleet_config):
            self.get_logger().info(f'initializing robot from \
                                   {api_client.configuration.host}')
            robot = Robot(self)
            robot.api = mir100_client.DefaultApi(api_client)

            # temporary retry configuration to workaround startup race condition while launching
            connection_pool_kw = robot.api.api_client.rest_client.pool_manager.connection_pool_kw
            orig_retries = connection_pool_kw.get('retries')
            retries = urllib3.Retry(10)
            retries.backoff_factor = 1
            retries.status_forcelist = (404, )
            connection_pool_kw['retries'] = retries

            mir_status = robot.api.status_get()
            robot.name = mir_status.robot_name

            self.load_missions(robot)
            self.update_positions(robot)
            # reset retries
            if orig_retries is not None:
                connection_pool_kw['retries'] = orig_retries
            else:
                del connection_pool_kw['retries']

            self.robots[robot.name] = robot
            self.get_logger().info(f'successfully initialized robot \
                                   {robot.name}')

        # Setup fleet driver ROS2 topic subscriptions
        self.path_request_sub = self.create_subscription(
            PathRequest, '/robot_path_requests', self.on_path_request, 1)
        self.mode_sub = self.create_subscription(ModeRequest,
                                                 f'/robot_mode_requests',
                                                 self.on_robot_mode_request, 1)
Пример #5
0
def compute_transforms(rmf_coordinates, mir_coordinates, node=None):
    """Get transforms between RMF and MIR coordinates."""
    transforms = {
        'rmf_to_mir': nudged.estimate(rmf_coordinates, mir_coordinates),
        'mir_to_rmf': nudged.estimate(mir_coordinates, rmf_coordinates)
    }

    if node:
        mse = nudged.estimate_error(transforms['rmf_to_mir'], rmf_coordinates,
                                    mir_coordinates)

        node.get_logger().info(f"Transformation estimate error: {mse}")
    return transforms
Пример #6
0
 def test_zero_error(self):
     dom = [(0, 0), (1, 1)]
     ran = [(1, 1), (2, 2)]
     t = nudged.estimate(dom, ran)
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 0.0)
Пример #7
0
 def test_zero_error(self):
     dom = [[0,0], [1,1]]
     ran = [[1,1], [2,2]]
     t = nudged.estimate(dom, ran)
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 0.0)
Пример #8
0
 def test_zero_error(self):
     dom = [[0, 0], [1, 1]]
     ran = [[1, 1], [2, 2]]
     t = nudged.estimate(dom, ran)
     mse = nudged.estimate_error(t, dom, ran)
     self.assertEqual(mse, 0.0)
def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time):
    # Profile and traits
    fleet_config = config_yaml['rmf_fleet']
    profile = traits.Profile(
        geometry.make_final_convex_circle(
            fleet_config['profile']['footprint']),
        geometry.make_final_convex_circle(fleet_config['profile']['vicinity']))
    vehicle_traits = traits.VehicleTraits(
        linear=traits.Limits(*fleet_config['limits']['linear']),
        angular=traits.Limits(*fleet_config['limits']['angular']),
        profile=profile)
    vehicle_traits.differential.reversible = fleet_config['reversible']

    # Battery system
    voltage = fleet_config['battery_system']['voltage']
    capacity = fleet_config['battery_system']['capacity']
    charging_current = fleet_config['battery_system']['charging_current']
    battery_sys = battery.BatterySystem.make(voltage, capacity,
                                             charging_current)

    # Mechanical system
    mass = fleet_config['mechanical_system']['mass']
    moment = fleet_config['mechanical_system']['moment_of_inertia']
    friction = fleet_config['mechanical_system']['friction_coefficient']
    mech_sys = battery.MechanicalSystem.make(mass, moment, friction)

    # Power systems
    ambient_power_sys = battery.PowerSystem.make(
        fleet_config['ambient_system']['power'])
    tool_power_sys = battery.PowerSystem.make(
        fleet_config['tool_system']['power'])

    # Power sinks
    motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
    ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
                                                 ambient_power_sys)
    tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)

    nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits)

    # Adapter
    fleet_name = fleet_config['name']
    adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter')
    if use_sim_time:
        adapter.node.use_sim_time()
    assert adapter, ("Unable to initialize fleet adapter. Please ensure "
                     "RMF Schedule Node is running")
    adapter.start()
    time.sleep(1.0)

    fleet_handle = adapter.add_fleet(fleet_name, vehicle_traits, nav_graph)

    if not fleet_config['publish_fleet_state']:
        fleet_handle.fleet_state_publish_period(None)
    # Account for battery drain
    drain_battery = fleet_config['account_for_battery_drain']
    recharge_threshold = fleet_config['recharge_threshold']
    recharge_soc = fleet_config['recharge_soc']
    finishing_request = fleet_config['task_capabilities']['finishing_request']
    node.get_logger().info(f"Finishing request: [{finishing_request}]")
    # Set task planner params
    ok = fleet_handle.set_task_planner_params(battery_sys, motion_sink,
                                              ambient_sink, tool_sink,
                                              recharge_threshold, recharge_soc,
                                              drain_battery, finishing_request)
    assert ok, ("Unable to set task planner params")

    task_capabilities = []
    if fleet_config['task_capabilities']['loop']:
        node.get_logger().info(
            f"Fleet [{fleet_name}] is configured to perform Loop tasks")
        task_capabilities.append(TaskType.TYPE_LOOP)
    if fleet_config['task_capabilities']['delivery']:
        node.get_logger().info(
            f"Fleet [{fleet_name}] is configured to perform Delivery tasks")
        task_capabilities.append(TaskType.TYPE_DELIVERY)
    if fleet_config['task_capabilities']['clean']:
        node.get_logger().info(
            f"Fleet [{fleet_name}] is configured to perform Clean tasks")
        task_capabilities.append(TaskType.TYPE_CLEAN)

    # Callable for validating requests that this fleet can accommodate
    def _task_request_check(task_capabilities, msg: TaskProfile):
        if msg.description.task_type in task_capabilities:
            return True
        else:
            return False

    fleet_handle.accept_task_requests(
        partial(_task_request_check, task_capabilities))

    # Transforms
    rmf_coordinates = config_yaml['reference_coordinates']['rmf']
    robot_coordinates = config_yaml['reference_coordinates']['robot']
    transforms = {
        'rmf_to_robot': nudged.estimate(rmf_coordinates, robot_coordinates),
        'robot_to_rmf': nudged.estimate(robot_coordinates, rmf_coordinates)
    }
    transforms['orientation_offset'] = \
        transforms['rmf_to_robot'].get_rotation()
    mse = nudged.estimate_error(transforms['rmf_to_robot'], rmf_coordinates,
                                robot_coordinates)
    print(f"Coordinate transformation error: {mse}")
    print("RMF to Robot transform:")
    print(f"    rotation:{transforms['rmf_to_robot'].get_rotation()}")
    print(f"    scale:{transforms['rmf_to_robot'].get_scale()}")
    print(f"    trans:{transforms['rmf_to_robot'].get_translation()}")
    print("Robot to RMF transform:")
    print(f"    rotation:{transforms['robot_to_rmf'].get_rotation()}")
    print(f"    scale:{transforms['robot_to_rmf'].get_scale()}")
    print(f"    trans:{transforms['robot_to_rmf'].get_translation()}")

    def _updater_inserter(cmd_handle, update_handle):
        """Insert a RobotUpdateHandle."""
        cmd_handle.update_handle = update_handle

    # Initialize robots for this fleet
    robots = {}
    for robot_name, robot_config in config_yaml['robots'].items():
        node.get_logger().info(f"Initializing robot:{robot_name}")
        rmf_config = robot_config['rmf_config']
        robot_config = robot_config['robot_config']
        initial_waypoint = rmf_config['start']['waypoint']
        initial_orientation = rmf_config['start']['orientation']
        robot = RobotCommandHandle(
            name=robot_name,
            config=robot_config,
            node=node,
            graph=nav_graph,
            vehicle_traits=vehicle_traits,
            transforms=transforms,
            map_name=rmf_config['start']['map_name'],
            initial_waypoint=initial_waypoint,
            initial_orientation=initial_orientation,
            charger_waypoint=rmf_config['charger']['waypoint'],
            update_frequency=rmf_config.get('robot_state_update_frequency', 1),
            adapter=adapter)

        if robot.initialized:
            robots[robot_name] = robot
            # Add robot to fleet
            fleet_handle.add_robot(robot, robot_name, profile, robot.starts,
                                   partial(_updater_inserter, robot))
            node.get_logger().info(
                f"Successfully added new robot:{robot_name}")

        else:
            node.get_logger().error(f"Failed to initialize robot:{robot_name}")

    return adapter, fleet_handle, robots
Пример #10
0
def main():
    #2, 62, 108
    path = '../datasets/dataset_house/'
    y1 = np.genfromtxt(path + 'house6.csv', delimiter=',')
    x1 = plt.imread(path + 'house.seq6.png')
    y2 = np.genfromtxt(path + 'house70.csv', delimiter=',')
    x2 = plt.imread(path + 'house.seq70.png')
    y3 = np.genfromtxt(path + 'house100.csv', delimiter=',')
    x3 = plt.imread(path + 'house.seq100.png')
    fig, ax = plt.subplots(1, 3)
    ax[0].imshow(x1)
    ax[0].plot(y1[:, 0], y1[:, 1], 'o', color='red')
    for i in range(len(y1)):
        ax[0].annotate(i, y1[i])
    ax[1].imshow(x2)
    ax[1].plot(y2[:, 0], y2[:, 1], 'o', color='red')
    for i in range(len(y2)):
        ax[1].annotate(i, y2[i])

    ax[2].imshow(x3)
    ax[2].plot(y3[:, 0], y3[:, 1], 'o', color='red')
    for i in range(len(y3)):
        ax[2].annotate(i, y3[i])

    #plt.show()
    mv = mview.basic([y1, y2, y3],
                     smart_initialize=True,
                     batch_size=10,
                     max_iter=700,
                     verbose=2)

    with open('output.csv', 'w') as fd:
        for each in mv.embedding:
            fd.write(",".join([str(each[0]),
                               str(each[1]),
                               str(each[2])]) + "\n")

    preds = np.array(mv.embedding)
    fig = plt.figure(figsize=plt.figaspect(.5))
    bx = fig.add_subplot(1, 2, 2, projection='3d')
    surf = bx.scatter(preds[:, 0] ,\
                      preds[:, 1],\
                      preds[:, 2],\
                      c='cyan',\
                      alpha=1.0,\
                      edgecolor='b')
    preds = mv.embedding

    #choice is the two points that chose to be connected
    choice = [(0, 1), (1, 2), (23, 24), (25, 26), (8, 9), (4, 5), (5, 6),
              (5, 15), (15, 17), (6, 16), (16, 18), (15, 16), (17, 18),
              (19, 21), (19, 20), (20, 22), (21, 22), (27, 28), (28, 29),
              (10, 11), (11, 13), (11, 12), (12, 14), (13, 14), (13, 29),
              (14, 29), (7, 11)]
    for each in choice:
        start = each[0]
        end = each[1]
        xs = preds[start][0], preds[end][0]
        ys = preds[start][1], preds[end][1]
        zs = preds[start][2], preds[end][2]
        line = plt3d.art3d.Line3D(xs, ys, zs)
        bx.add_line(line)
    #bx.plot([preds[0]], [preds[1]] , color='blue')
    '''
    #bx.plot3D(preds[1:3, 0], preds[1:3, 1], preds[1:3, 2], color='blue')
    #bx.plot3D(preds[23:25, 0], preds[23:25, 1], preds[23:25, 2], color='blue')
    #bx.plot3D(preds[25:27, 0], preds[25:27, 1], preds[25:27, 2], color='blue')
    #bx.plot3D(preds[8:10, 0], preds[8:10, 1], preds[8:10, 2], color='blue')
    #bx.plot3D(preds[4:6, 0], preds[4:6, 1], preds[4:6, 2], color='blue')
    bx.plot3D(preds[5:7, 0], preds[5:7, 1], preds[5:7, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[5:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue')
    #bx.plot3D(preds[0:4, 0], preds[0:4, 1], preds[0:4, 1], color='blue')
    '''
    mv.plot_embedding()
    mv.plot_computations()
    mv.plot_images(labels=True)

    projections = []
    projections.append(mv.embedding @ mv.projections[0].T)
    projections.append(mv.embedding @ mv.projections[1].T)
    projections.append(mv.embedding @ mv.projections[2].T)

    trans = []
    trans.append(nudged.estimate(y1, projections[0]))
    trans.append(nudged.estimate(y2, projections[1]))
    trans.append(nudged.estimate(y3, projections[2]))

    fig1, cx = plt.subplots(1, 3)

    Z = [x1, x2, x3]
    Y = [y1, y2, y3]
    for i in range(len(trans)):
        each = trans[i]
        m = each.get_matrix()
        r = each.get_rotation()
        s = each.get_scale()
        t = each.get_translation()
        #s = 1.0

        #print(rotate(Y[i], r))
        mse = nudged.estimate_error(each, Y[i], projections[i])
        print(i, 'error:', mse, 'scale:', s)

        do_plot(
            cx[i], Z[i],
            mtransforms.Affine2D().rotate(r).scale(s).translate(t[0], t[1]),
            projections[i])

    plt.show()
    input()