Package lejos.robotics

Examples of lejos.robotics.Pose

82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
    mcl = new MCLPoseProvider(pilot,this, map, NUM_PARTICLES, BORDER);
    particles = mcl.getParticles();
    particles.setDebug(true);
   
    // Make random moves until we know where we are
    Pose start = localize();
 
    // Find a route home
    Pose home = new Pose(50, 300, -90);
    PathFinder pf = new MapPathFinder(map, readings);
    PoseController pc = new  ArcPoseController(pilot, mcl);
   
    System.out.println("Located: (" + (int) start.getX() + "," + (int) start.getY() + ")");
   
    // Go home
    try {
      Collection<WayPoint> route = pf.findRoute(start, home);
     
      for(WayPoint wp: route) {
        System.out.println("Go to (" + (int) wp.x + "," + (int) wp.y + ")");
        Pose pose = pc.goTo(wp);
        goodEstimate(pose); // Just for diagnostics
        // Pose controller should have a goTo(Pose) method to do this
        pilot.rotate(wp.getHeading() - pose.getHeading());
      }
    } catch (DestinationUnreachableException e) {
      System.out.println("Unreachable");
    }
    Button.waitForPress();
View Full Code Here
179
180
181
182
183
184
185
186
187
188
  /**
   * Make random moves until the estimated pose is good enough
   */
  private Pose localize() {
    for(;;) {
      Pose pose = mcl.getPose();
      if (goodEstimate(pose)) return pose;
      else randomMove();
    }
  }
View Full Code Here
70
71
72
73
74
75
76
77
78
79
    g2d.setColor(Color.red);
    for (int i = 0; i < numParticles; i++) {
      MCLParticle part = particles.getParticle(i);
      if (part != null) {
        if (i == closest) g2d.setColor(Color.green);
        paintPose(g2d, new Pose(part.getPose().getX(), part.getPose().getY(), part.getPose().getHeading()));
        g2d.setColor(Color.red);
      }
    }   
  }
View Full Code Here
114
115
116
117
118
119
120
121
122
123
124
  private void paintLocation(Graphics2D g2d) {
    float minX = particles.getMinX();
    float maxX = particles.getMaxX();
    float minY = particles.getMinY();
    float maxY = particles.getMaxY();
    Pose estimatedPose = particles.getEstimatedPose();
    if (maxX - minX > 0 && maxX - minX <= MAX_CLUSTER_SIZE &&
        maxY - minY > 0 && maxY - minY <= MAX_CLUSTER_SIZE) {
        Ellipse2D c = new Ellipse2D.Float(X_OFFSET + minX * PIXELS_PER_CM, Y_OFFSET + minY * PIXELS_PER_CM, (maxX - minX)  * PIXELS_PER_CM, (maxY - minY)  * PIXELS_PER_CM);
        g2d.setColor(Color.blue);
        g2d.draw(c);
View Full Code Here
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
    return null;
  }

  public Collection<WayPoint> findRoute(Pose start, Pose destination)
      throws DestinationUnreachableException {
    Pose pose = start;
   
    // Continue until we return a route or throw DestinationUnReachableException
    for(;;) {
      // If the current pose if close enough to the destination, go straight there
      if (pose.distanceTo(destination.getLocation()) < MAX_DISTANCE) {
        add(new WayPoint(destination));
        return this;
      } else {
        Pose testPose = null;
       
        // Generate random poses and apply tests to them
        for(int i=0;i<MAX_ITERATIONS;i++) {
            testPose = generatePose();
           
            // The new Pose must not be more than MAX_DISTANCE away from current pose     
            if (testPose.distanceTo(pose.getLocation()) > MAX_DISTANCE) continue;
           
          // The new pose must be at least MIN_GAIN closer to the destination
          if (pose.distanceTo(destination.getLocation()) -
              testPose.distanceTo(destination.getLocation()) < MIN_GAIN)
            continue;
         
          // We must be able to get a valid set of range readings from the new pose
          float heading = testPose.getHeading();
          boolean validReadings = true;
          for(RangeReading r: readings) {
            testPose.setHeading(heading + r.getAngle());
            float range = map.range(testPose);
            if (range > MAX_RANGE) {
              validReadings = false;
              break;
            }
          }         
          if (!validReadings) continue;
         
          //Check there are no obstacles in the way
          testPose.setHeading(testPose.angleTo(pose.getLocation()));
          if (map.range(testPose) < testPose.distanceTo(pose.getLocation()))
            continue;         
         
          testPose.setHeading(heading); // Restore heading
          break; // We have a new way point
        }
        if (testPose == null) throw new  DestinationUnreachableException();
        else {
          add(new WayPoint(testPose));
View Full Code Here
121
122
123
124
125
126
127
      }

      // Pick a random angle
      heading = ((float) Math.random()) * 360;
     
      return new Pose(x,y,heading);
  }
View Full Code Here
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
  public void setPoseProvider(PoseProvider replacement) {
    poseProvider = replacement;
  }

  public Pose goTo(Point destination) {
    Pose pose = poseProvider.getPose();
    if (pilot instanceof RotatePilot) { // optimize for RotatePilot
        float turnAngle = pose.angleTo(destination) - pose.getHeading();
        while (turnAngle < -180)turnAngle += 360;
        while (turnAngle > 180) turnAngle -= 360;
      ((RotatePilot) pilot).rotate(turnAngle);
      pilot.travel(pose.distanceTo(destination));     
    } else {
      //TODO: Calculate arc of minimum radius needed to point to the
      // destination and the do an arc and a travel.
    }
    return poseProvider.getPose();
View Full Code Here
39
40
41
42
43
44
45
    mp.addMoveListener(this);
  }
   
  public Pose getPose() {
    if (!current) updatePose(mp.getMovement());
    return new Pose(x,y,heading);
  }
View Full Code Here
49
50
51
52
53
54
55
  public void setMaxHeadingError(float distance) {
    maxHeadingError = distance;
  }
 
  public Pose getPose() {
    return new Pose(x,y,heading);
  }
View Full Code Here
TOP

Related Classes of lejos.robotics.Pose

  • Homer
  • lejos.geom.Point
  • lejos.pc.comm.MCLFrame
  • lejos.robotics.proposal.ArcPoseController
  • lejos.robotics.proposal.DeadReckonerPoseProvider
  • lejos.robotics.proposal.MapPathFinder
  • lejos.robotics.proposal.WayPoint
    • Copyright © 2018 www.massapicom. All rights reserved.
    All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.