原文：Beginning Robotics Programming in Java with LEGO Mindstorms
四、协调器和导航器 API
这一章向你介绍了在莱霍斯 EV3 使用的笛卡尔坐标系的基础知识。它还教你如何在导航课程中应用编程方法来控制轮式车辆，以便在二维平面中用坐标描绘出预定义的路径。此外，您将了解乐高 EV3 积木的主要硬件组件，如液晶显示器和与机器人互动的按键。您还将学习如何应用用于控制 LCD 显示器的方法以及用于向机器人提供输入和从机器人获得输出的按钮。
特别是，本章包括七个 Java 项目示例，并涵盖以下主题:
- 笛卡尔坐标系简介
- 导航器 API 函数的基础
- 控制 EV3 砖硬件
- 按钮和液晶显示器编程练习
- 追踪二维平面的编程实践
笛卡尔坐标系基础
从我们的角度来看，我们可以很容易地用诸如“我位于第 10 大道和第 2 街的拐角处”或“我位于新罕布什尔州基恩市主街 100 号”这样的词语来描述位置然而，这样的描述对乐高机器人没有任何意义，因为它们没有语义概念。相反，乐高机器人只理解数字。
因此，当编程乐高机器人时，笛卡尔坐标系被用来描述位置。如图 4-1 所示，一个二维笛卡尔坐标系记录两个数字:X 和 Y 的值，数字沿着 X 轴和 Y 轴变大变小。两个轴都从 0 开始，包含正数和负数。X 轴和 Y 轴将笛卡尔坐标系分成四个象限，即区域 I、II、III 和 IV。二维区域中的任何一点都可以用 X 和 y 的值绘制在这个网格上。
图 4-1。
A Cartesian coordinate system
此外，在笛卡尔坐标系中，向左旋转被指定为正旋转。这意味着，如果你旋转正 90 度(+90)，你已经逆时针旋转。同样，负 90 度(-90°)的旋转相当于顺时针方向旋转。
导航 API 函数
在第三章中，你学习了飞行员，比如MovePilot和DifferentialPilot，以及飞行员如何让机器人执行精确的动作和行驶特定的距离。使用 pilot，您将了解另一个名为 Navigator 的类，它告诉飞行员如何根据笛卡尔坐标系驾驶到特定的位置。
导航器在其构造函数中接受一个 pilot 对象，然后计算从一个位置到另一个位置的一系列移动。导航者实际上对机器人如何工作或者飞行员物体如何四处移动一无所知。取而代之的是，导航器只是要求飞行员执行运动指令。
下面的代码语句实例化一个导航器，然后行驶到目标坐标 x = 50，y = 50，其中我们假设存在一个导航器:
Navigator navtest = new Navigator(pilot);
navtest.goTo(50,50);
正如您在第三章中了解到的，您可以使用以下程序构建一个 pilot 对象:
// setup the wheel diameter of left (and right) motor in
// centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center of
// wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
在导航类中，目标坐标也是已知的航路点。您可以创建自己的方法来生成一组路点，然后使用路点对象将坐标提供给导航器。以下内容显示了如何使用该航路点:
Waypoint wp = new Waypoint (50, 50);
navtest.goTo(wp);
在查找路径的情况下，一旦找到路径，您就可以使用addWaypoint()方法将路点放入导航队列。例如，
navtest.addWaypoint(new Waypoint(200,200));
导航器驱动导航对象进行移动控制。这是通过PoseProvider来实现的，它跟踪机器人在一条路线上的位置顺序。route 是 path 类的一个实例，它表现为一个先进先出(FIFO)队列。当到达一个航路点时，它会从路线队列中删除，机器人会转到下一个航路点。在任何时候，新的路点都可以添加到路线队列的末尾。
下面是导航类中的方法列表。导航类提供的所有方法都是非阻塞的，这意味着这些方法在完成它们的功能后会立即返回。
void setPath(Path path)设置导航器遍历的路径。void addWaypoint(Waypoint waypoint)在路径末端添加一个航路点。void addWaypoint(float x, float y )构建一个坐标为(x，y)的新航路点，然后将其添加到路径末端。void addWaypoint(float x, float y , heading )创建一个坐标为(x，y)的新航路点，然后将其添加到路径末端。当机器人到达航路点时，您可以使用此方法指定机器人的方向。void followPath()启动机器人沿现有/当前路线移动。void followPath(Path path )启动机器人遍历要跟随的路径。void goTo(Waypoint destination)启动机器人向目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，目标将被添加到现有路径中。void goTo(double x, double y)启动机器人向坐标(x，y)指定的目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，新的航路点将被添加到现有路径中。void goTo(double x, double y, double heading)启动机器人向坐标(x，y)指定的目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，新的航路点将被添加到现有路径中。当机器人到达目的地航路点时，您可以使用此方法指定机器人的方向。leJOSvoid stop()停止机器人，但保留路线，以便您可以在调用followPath()时恢复其路径遍历。- 如果机器人正在向一个航路点移动，返回 true。
- 如果机器人已经到达最后一个航路点，返回 true。
void rotateTo(double direction)将机器人旋转到笛卡尔平面中新的方向角度。例如，当 x 轴为 0 时，rotateTo(0)会将机器人与 x 轴对齐。如果 y 轴是 90 度，rotateTo(90)将其与 y 轴对齐。方向的数值是机器人可以旋转到的绝对方向，范围从 0 到 360。void waitForStop()如果机器人停在路径的最后一个航路点并且不再移动，返回 true。void singleStep(boolean yes)控制机器人是否停在每个航路点。如果yes，你可以用真参数调用这个方法，然后机器人停在每个航点。
下面是一个简单的例子，ch4p1.java，将你的机器人从起点(0，0)移动到坐标为(50，50)的位置。这里的测量单位是厘米，因为 pilot 使用的直径和轨道宽度是厘米(例如，2.8 厘米和 18 厘米)。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p1.java
//an example for navigation testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class ch4p1 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
Navigator navtest = new Navigator(pilot);
// define a new waypoint as destination
Waypoint wp = new Waypoint (50, 50);
// robot moves to the destination waypoint
navtest.goTo(wp);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
控制 EV3 砖硬件
EV3 砖配备了 6 节 AA 电池，或稍大的可充电电池组。这种耐用的砖块还包含一个 32 位 ARM9 处理器，运行频率为 300MHz。该处理器可直接访问 64MB RAM 和 16MB 闪存。为了延长电池寿命，即使没有电源输入，闪存也能存储数据和程序。与拥有 4GB 内存的现代计算机相比，乐高 EV3 使用的内存非常小。然而，这对您的机器人项目来说实际上已经足够了，因为现代计算机消耗了大量的 RAM 容量来运行操作系统、生成图形和执行其他内存密集型任务。然而，机器人程序不使用重型图形或执行处理器密集型计算任务。
在本节中，您将熟悉 EV3 砖，包括用于输入的按钮、液晶显示器(LCD)和用于输出的小型扬声器。莱霍斯 EV3 公司提供用于控制所有硬件的 API(应用接口)功能。
莱霍斯 EV3 的 LCD 类可用于文本模式或图形模式。对于文本显示，EV3 液晶显示屏宽 16 个字符，深 8 个字符。用坐标(x，y)来寻址，如图 4-2 所示，其中 x 的范围是 0 到 15，y 的范围是 0 到 7。
图 4-2。
Coordinate system of LCD display
在文本模式下写入 LCD 屏幕的方法如下:
void drawString(String str, int x, int y)从坐标(x，y)开始，在 LCD 屏幕上显示一串文本。void drawInt(int i, int x, int y)显示从坐标(x，y)开始的整数。整数是左对齐的，根据需要占用尽可能多的字符。void drawInt(int i, int places, int x, int y)显示一个从坐标(x，y)开始的整数，其前导空格至少占据 places 参数指定的字符数。这意味着该方法总是写入位置中指定的固定数量的字符，并且以前的值将总是被完全覆盖。例如，如果 places 的值设置为 5，这意味着在 LCD 上显示整数时有 5 个字符要覆盖。void clear()清除显示。
作为一个例子，程序ch2p2. java说明了如何在 EV3 砖的 LCD 上显示空闲存储空间:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p2.java
//An example to test LCD displaying methods
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
public class ch4p2 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantized LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// drawing text on the LCD screen based on coordinates
lcddisplay.drawString("Free RAM:", 0, 0);
// displaying free memory on the LCD screen
lcddisplay.drawInt((int) Runtime.getRuntime().freeMemory(), 0, 1);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
您也可以用System.out.println(String str)写入 LCD 显示屏。例如，System.out.println("hello")将在屏幕上显示字符串hello，并覆盖现有字符。默认情况下，LCD 显示屏会自动刷新。如果你想控制 LCD 何时刷新，你可以调用lcddisplay.setAutoRefresh(false)关闭自动刷新，当你想刷新显示时调用lcddisplay.refresh()。
EV3 砖块上的所有六个控制键都可以在莱霍斯 keys 的控制下重新编程。您可以使用事件来监听按键，并在按键被激活时做出相应的反应。Keys类包含六个键的静态实例。这六个实例是ENTER、ESCAPE、LEFT、RIGHT、UP和DOWN。
通常，在编程时，您希望等待或阻止进程，直到某个键被按下。因此，最简单的方法就是使用waitForAnyPress()方法。例如，要停止代码直到按下ESCAPE键，您可以执行以下操作:
buttons.getButtons()==Keys.ID_ESCAPE
您还可以使用一个简单的 while 循环来停止您的代码，同时等待用户按下一个ESCAPE按钮:
while(buttons.getButtons() != Keys.ID_ESCAPE) { }
以下程序ch4p3.java测试按钮是否被按下:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p3.java
//an example for button testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
public class ch4p3 {
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// Keys class for buttons
Keys buttons = ev3brick.getKeys();
// press the ESCAPE key to exit the program
while (buttons.getButtons() != Keys.ID_ESCAPE) {
// clearing the LCD screen at first
LCD.clear();
// press the ENTER key so the ENTER will be
// displayed on the LCD screen
if (buttons.getButtons() == Keys.ID_ENTER) {
// displaying ENTER on the LCD screen
LCD.drawString("ENTER", 0, 0);
// leave the string ENTER on the screen
// for 2 seconds
Thread.sleep(2000);
}
}
}
}
用于控制按键的其他方法包括:
static int waitForAnyPress()
要等待任何键被按下，上面的命令返回被按下的键的 id 代码，如下所示:
static int readButtons()读取所有按键的当前状态。返回值是被按下的键的所有代码的总和。
如上面的程序ch4p3.java所示，你会发现Thread.sleep(2000)是用来加入各种延迟，以便机器人可以休息去做其他事情。在许多情况下，当使用传感器(例如，光传感器或颜色传感器)记录需要极高精度的快速事件时，会发生这种情况。这样的事件可能是等待颜色传感器检测黑线，或者等待超声波传感器检测墙壁。因此，方法Thread.sleep()可以用于为不同的动作计时或者为事件打上时间戳，以供以后分析。
液晶显示器编程实践
在本练习中，您将开发一个程序，在 LCD 屏幕上显示“Here is my RAM”五秒钟，然后打印出“I got it.”。一个示例程序ch4p4.java说明了如何实现这一目标。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p4.java
//A programming practice example to display free RAM on LCD
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
import lejos.utility.Stopwatch;
public class ch4p4 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// for timing dialogs
Stopwatch sw = new Stopwatch();
// drawing free RAM on the LCD screen based on
// coordinates
lcddisplay.drawString("Here is my RAM:", 0, 0);
// displaying free memory on the LCD screen
lcddisplay.drawInt((int) Runtime.getRuntime().freeMemory(), 0, 1);
sw.reset();
// wait for 5 seconds, then display a message
while (sw.elapsed() < 5000)
Thread.yield();
sw.reset();
lcddisplay.drawString("I got it", 0, 2);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
按键编程练习
在本编程练习课中，您将开发两个程序来按键并在 LCD 上显示相应的消息。在第一个程序中，您需要在 LCD 上显示被按下的按钮。其伪代码如下例所示:
while(ESCAPSE is not pressed){
Clear LCD
If ENTER is pressed
Display “ENTER” on first row
Else if LEFT is pressed
Display “LEFT” on the first row
else if RIGHT is pressed
Display “RIGHT” on the first row
else if UP is pressed
Display “UP” on the first row
else if DOWN is pressed
Display “DOWN” on the first row
}
一个示例程序ch4p5.java向您展示了如何完成这个练习。
//************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p5.java
//A programming practice example to display various buttons when //they are pressed
//************************************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
public class ch4p5 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// continue waiting for button pressed while the user // has not pressed
// escape button
while (buttons.getButtons() != Keys.ID_ESCAPE) {
// display a message for enter
if (buttons.getButtons() == Keys.ID_ENTER) {
lcddisplay.clear();
lcddisplay.drawString("ENTER", 0, 0);
}
// display a message for left button
else if (buttons.getButtons() == Keys.ID_LEFT) {
lcddisplay.clear();
lcddisplay.drawString("LEFT", 0, 0);
}
// display a message for right button
else if (buttons.getButtons() == Keys.ID_RIGHT) {
lcddisplay.clear();
lcddisplay.drawString("RIGHT", 0, 0);
}
// display a message for up button
else if (buttons.getButtons() == Keys.ID_UP) {
lcddisplay.clear();
lcddisplay.drawString("UP", 0, 0);
}
// display a message for down button
else if (buttons.getButtons() == Keys.ID_DOWN) {
lcddisplay.clear();
lcddisplay.drawString("DOWN", 0, 0);
}
}
}
}
在第二个练习中，您需要编写一个程序，在执行程序的其余部分之前等待ENTER被按下。下面的示例说明了伪代码:
Clear LCD
Display "Press ENTER to continue" then Refresh LCD
Wait for the ESCAPE to be pressed and released
Display a message that the ESCAPE is indeed pressed and released
一个示例程序ch4p6.java向您展示了如何完成这个练习。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p6.java
//A programming practice example to display ESCAPE button pressed
// and released
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
import lejos.utility.Stopwatch;
public class ch4p6 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// print instructions for the user
lcddisplay.clear();
lcddisplay.drawString("Prs ENTER to cont", 0, 0);
lcddisplay.refresh();
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// wait until ESCAPE key is pressed, then wait for it // to be released
while (buttons.getButtons() != Keys.ID_ESCAPE)
Thread.yield();
// once escape is released, indicate that it was and
// pause for a moment
// (i.e. 5 seconds) before exiting
lcddisplay.drawString("Escape was pressed", 0, 1);
Stopwatch sw = new Stopwatch();
while (sw.elapsed() < 5000)
Thread.yield();
}
}
Navigator API 编程实践
leJOS Navigator 界面提供了一组方法，可用于精确控制机器人的运动，包括移动到任何位置和控制运动方向的方法。在这个练习环节中，你将编写一个程序，让机器人在二维平面内行走，如图 4-3 所示:
图 4-3。
A two-dimensional plane for testing navigator API
starting at A(0, 0),
go to B(50, 50),
then, go back A(0, 0)
then go to C(-50, 50),
then finally come back to A(0, 0).
Note
在本练习课中，坐标的单位是厘米。因此，你必须确保你的轨道宽度和直径设置为厘米。
该程序的伪代码如下:
DifferentialPilot ev3robot = new DifferentialPilot(diam,trackwidth,Motor.A,Motor.C);
NavPathController navbot = new NavPathController(ev3robot);
Button waitForPress to start
Display destination’s coordinate (50, 50) on LCD
Display the message "Press ENTER key" to continue; Go to location with coordinate 50,50
Display destination’s coordinate (0, 0) on LCD
Display the message "Press ENTER key" to continue; Go to location with coordinate 0,0
Display destination’s coordinate (-50, 50) on LCD Display the message "Press ENTER key" to continue; Go to location with coordinate -50,50
Display destination’s coordinate (0, 0) on LCD Display the message "Press ENTER key" to continue; Go to location with coordinate 0,0
Button waitForPress to exit
下面的示例程序ch4p7.java向您展示了如何完成这个练习。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p7.java
//A programming example to practice navigator API functions
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.TextLCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.utility.TextMenu;
public class ch4p7 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws InterruptedException {
// menu options for displaying navigator controlling
String[] menuItems = new String[1];
menuItems[0] = "Task 1: Navigator";
// display menu
TextMenu menu = new TextMenu(menuItems);
menu.setTitle("NavRobot");
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// run routine based on menu choice
switch (menu.select()) {
case 0:
navigate(navbot);
break;
}
}
// navigate()
// demo navigating using way points
private static void navigate(Navigator nav) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// set up way points
Waypoint wp1 = new Waypoint(50, 50);
Waypoint wp2 = new Waypoint(0, 0);
Waypoint wp3 = new Waypoint(-50, 50);
// clear menu
lcddisplay.clear();
// display current location
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point one, display new location
lcddisplay.clear();
nav.goTo(wp1);
lcddisplay.drawString("At 50, 50", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point two, display new location
lcddisplay.clear();
nav.goTo(wp2);
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point 3, display new location
lcddisplay.clear();
nav.goTo(wp3);
lcddisplay.drawString("At -50, 50", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point 2, display new location
lcddisplay.clear();
nav.goTo(wp2);
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to exit", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
}
}
在图 4-3 所示的笛卡尔坐标系中已经看到，节点 A 的坐标为(0，0)，节点 B 的坐标为(50，50)，节点 C 的坐标为(-50，50)。机器人的路径从节点 A 开始，遍历到节点 B，然后回溯到节点 A，遍历到节点 C，最后回溯到节点 A。节点 A、B 和 C 实际上形成了二叉树中的小元素，并且该实践显示了遍历二叉树的最简单形式。
摘要
在这一章中，你学习了 EV3 使用的笛卡尔坐标系统。您还学习了如何在导航类中应用各种方法来控制轮式车辆，以便它们可以在二维平面中用坐标描绘出预定义的路径。此外，您还了解了 EV3 砖块的主要硬件组件:即 LCD 显示屏和 EV3 砖块上用于与机器人交互的六个按键。最后，您学习了应用内置系统方法来控制 LCD 显示器和按钮，以便向机器人提供输入和/或从机器人获得输出。
在下一章，你将进入本书的下一部分，也就是说，通过使用搜索策略为你的机器人建立智能。特别是，将学习如何在小型 Java 虚拟机(JVM)上实现深度优先搜索(DFS)算法，即 LeJOS EV3，然后将 DFS 算法集成到基于 LeJOS 的机器人系统中进行定位和路径规划，增强 LeJOS 系统中现有的寻路方法。
五、深度优先搜索算法及其在乐高 EV3 中的实现
搜索行为属于人工智能(AI)范畴。人工智能的主要目标是赋予计算机思考的能力:换句话说，就是模仿人类的行为。不幸的是，这种模仿的问题在于，计算机的“大脑”并不像人脑一样工作:计算机需要一系列合理的步骤来处理，以找到解决方案。因此，你的目标是解构一个复杂的任务，并将其转化为机器人系统可以处理的简单步骤。这种从复杂到简单的转换，就是搜索算法要做的。
在这一章中，你将学习如何在小型 Java 虚拟机(JVM)上实现深度优先搜索(DFS)算法:当然，这就是莱霍斯·EV3。众所周知，leJOS EV3 是一个开源项目，旨在开发一个技术基础设施，将 Java 技术应用于机器人编程软件。Java 是一种面向对象的编程语言。leJOS 中实现的最重要的功能之一是 leJOS 导航 API(在第四章中讨论)，它可以用来实现一个目标，其中提供了一组方便的类和方法来控制机器人。
控制车辆的类处理几个层次的抽象。在最底部，有转动轮子的马达，由RegulatedMotor级控制。MovePilot(或 leJOS NXJ 中的DifferentialPilot)类使用电机控制基本动作:原地旋转、直线行进或弧形行进。在下一个级别，NavPathController使用DifferentialPilot来移动机器人通过平面上的复杂路径。为了执行导航，路径控制器需要机器人的位置和它前进的方向。它使用一个OdometeryPoseProvider来保持这个信息是最新的。特别是，本章将涵盖以下主题:
- 一种新的深度优先搜索(DFS)算法，可用于构建任意树结构。
- 在基于 leJOS 的机器人系统中应用和集成提出的 DFS 算法用于定位和路径规划，在该过程中增强 leJOS 系统中现有的寻路方法。
DFS 算法概述
我们先来考察一下人类是如何解决搜索问题的。首先，需要描述搜索问题是如何存在的。图 5-1 是一个搜索树的例子。这是一系列相互连接的节点，我们将通过这些节点进行搜索:
图 5-1。
Tree structure of a path
深度优先搜索(DFS)的工作原理是取一个节点；检查它的邻居；扩展它在那些邻居中找到的第一个节点；检查展开的节点是否是您的目的地；如果没有，继续探索更多的节点。例如，如果你想找到一条从 A 到 E 的路径，你可以使用两个列表来跟踪你在做什么:一个开放列表和一个封闭列表。开放列表记录你需要做什么，封闭列表记录你已经做了什么。
开始的时候，你只有你的起点，节点 a，你还没对它做什么，那就把它加入你的开放列表吧。然后你有一个包含<A>的开放列表和一个包含<empty>的封闭列表。现在让我们探索一下你的 A 节点的邻居。节点 A 的邻居是 B、C 和 D 节点。因为您现在已经完成了您的 A 节点，所以您可以将它从您的开放列表中移除，并将其添加到您的封闭列表中。您当前的开放列表包含<B, C, D>，而封闭列表包含<A>。现在我们的开放列表包含三个项目。
对于深度优先搜索，您总是从开放列表中探索第一个项目。开放列表中的第一项是 B 节点。b 不是你的目的地，我们来探索一下它的邻居。因为您现在已经扩展了 B，所以您将把它从开放列表中移除，并将其添加到封闭列表中。您的新节点是 E、F 和 G，您将这些节点添加到开放列表的开头。然后你有一个包含<A, B>,的开放列表和一个包含<E, F, G, C, D>的封闭列表。现在展开 E 节点。既然是你预定的目的地，就应该停下来。因此，您会收到通过使用常规的深度优先搜索算法从封闭列表中解释的路由A->B->E。
接下来，让我们看看如何使用深度搜索方法来解决路径定位问题，该问题可以应用于从起始城市导航到目的城市的 GPS 系统。假设您想从城市 A(例如新罕布什尔州的基恩)开车到城市 S(比如马萨诸塞州的波士顿)。给定以下路线，使用深度优先搜索策略，确定一个从 A 开始到 S 结束的计划。
| 城市 | 距离 | | --- | --- | | 从 a 到 B | 20 英里 | | 从 a 到 C | 10 英里 | | 从 a 到 D | 10 英里 | | 从 a 到 E | 20 英里 | | （back to back）背对背 | 10 英里 | | b 到 M | 20 英里 | | b 到 G | 10 英里 | | c 到 H | 10 英里 | | c 到 I | 15 英里 | | 从 d 到 J | 20 英里 | | e 到 K | 15 英里 | | e 到 L | 15 英里 | | m 到 N | 20 英里 | | 摩托 g | 20 英里 | | 从 I 到 P | 40 英里 | | 个人对个人 | 20 英里 | | 个人对个人 | 20 英里 |基于以上信息，你可以绘制一个路径树，如图 5-2 所示，显示两个城市之间所有可能的路径:
图 5-2。
Routes in between two cities
以下程序是一个示例代码，可用于通过使用深度优先搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_main.java
//Driver class to set up map using ch5p1_GraphNode, ch5p1_Link, and //ch5p1_Graph classes.
//Calls a depth-first search in ch5p1_Graph class to create //navigation path from a start
//and end node.
//******************************************************************
public class ch5p1_main {
public static void main(String[] args) {
// These objects used to define what your graph looks // like
ch5p1_Graph searchGraph = new ch5p1_Graph();
ch5p1_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M, N, O, P, R, S;
ch5p1_Link AB, AC, AD, AE, BF, BM, BG, CH, CI, DJ, EK, EL, MN, MO, IP, PR, PS;
// define each node
A = new ch5p1_GraphNode("A");
B = new ch5p1_GraphNode("B");
C = new ch5p1_GraphNode("C");
D = new ch5p1_GraphNode("D");
E = new ch5p1_GraphNode("E");
F = new ch5p1_GraphNode("F");
G = new ch5p1_GraphNode("G");
H = new ch5p1_GraphNode("H");
I = new ch5p1_GraphNode("I");
J = new ch5p1_GraphNode("J");
K = new ch5p1_GraphNode("K");
L = new ch5p1_GraphNode("L");
M = new ch5p1_GraphNode("M");
N = new ch5p1_GraphNode("N");
O = new ch5p1_GraphNode("O");
P = new ch5p1_GraphNode("P");
R = new ch5p1_GraphNode("R");
S = new ch5p1_GraphNode("S");
// define which GraphNodes are connected
AB = new ch5p1_Link(A, B);
AC = new ch5p1_Link(A, C);
AD = new ch5p1_Link(A, D);
AE = new ch5p1_Link(A, E);
BF = new ch5p1_Link(B, F);
BM = new ch5p1_Link(B, M);
BG = new ch5p1_Link(B, G);
CH = new ch5p1_Link(C, H);
CI = new ch5p1_Link(C, I);
DJ = new ch5p1_Link(D, J);
EK = new ch5p1_Link(E, K);
EL = new ch5p1_Link(E, L);
MN = new ch5p1_Link(M, N);
MO = new ch5p1_Link(M, O);
IP = new ch5p1_Link(I, P);
PR = new ch5p1_Link(P, R);
PS = new ch5p1_Link(P, S);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addNode(N);
searchGraph.addNode(O);
searchGraph.addNode(P);
searchGraph.addNode(R);
searchGraph.addNode(S);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(AE);
searchGraph.addLink(BF);
searchGraph.addLink(BM);
searchGraph.addLink(BG);
searchGraph.addLink(CH);
searchGraph.addLink(CI);
searchGraph.addLink(DJ);
searchGraph.addLink(EK);
searchGraph.addLink(EL);
searchGraph.addLink(MN);
searchGraph.addLink(MO);
searchGraph.addLink(IP);
searchGraph.addLink(PR);
searchGraph.addLink(PS);
// run depth-first search to get from start to
// destination
searchGraph.dfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(S)));
// display path created using dfsTraverse
// This will be display the path from start to
// destination
System.out.println("the path from your current city to the destination city is: ");
for (int i = searchGraph.dfsPath.size() - 1; i >= 0; i--) {
if(i!=0)
System.out.print(searchGraph.dfsPath.get(i).cityName + "->");
else System.out.print(searchGraph.dfsPath.get(i).cityName);
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
//ch5p1_GraphNode.java
//Represents name of a graph
//******************************************************************
public class ch5p1_GraphNode {
String cityName;
public ch5p1_GraphNode(String cityName) {
this.cityName = cityName;
}
public String toString() {
return cityName;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a depth-first
//search traversal of the graph.
//the method dfsTraverse creates a dfsPath list which the robot will //follow to demonstrate
//how the depth-first search works.
//******************************************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch5p1_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch5p1_GraphNode> nodes;
ArrayList<ch5p1_Link> links;
// Two lists used for traversing
ArrayList<ch5p1_GraphNode> dfsTraverse;
Stack<ch5p1_Link> travelStack = new Stack<ch5p1_Link>();
// List used to define where you would like to move
ArrayList<ch5p1_GraphNode> dfsPath = new ArrayList<ch5p1_GraphNode>();
// Constructor of ch5p1_Graph class
public ch5p1_Graph() {
nodes = new ArrayList<ch5p1_GraphNode>();
links = new ArrayList<ch5p1_Link>();
dfsTraverse = new ArrayList<ch5p1_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch5p1_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch5p1_Link link) {
links.add(link);
}// end addLink
// dfsTraverse()
// perform depth-first search on the graph
public void dfsTraverse(ch5p1_GraphNode from, ch5p1_GraphNode to) {
boolean matched;
ch5p1_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to dfsPath
// This will ultimately repeated by the end of the
// search
matched = match(from, to);
if (matched) {
travelStack.push(new ch5p1_Link(from, to));
dfsPath.add(new ch5p1_GraphNode(to.cityName));
dfsPath.add(new ch5p1_GraphNode(from.cityName));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to dfsPath
// recursively call dfsTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch5p1_Link(from, to));
dfsTraverse(found.to, to);
dfsPath.add(new ch5p1_GraphNode(from.cityName));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
dfsTraverse(found.from, found.to);
dfsPath.add(new ch5p1_GraphNode(from.cityName));
}
}// end dfsTraverse()
// find() method is used to
// find the next link to try exploring
public ch5p1_Link find(ch5p1_GraphNode from) {
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
// link found
if (links.get(a).from.equals(from) && !links.get(a).skip) {
ch5p1_Link foundList = new ch5p1_Link(links.get(a).from,
links.get(a).to);
// mark this link as used so we don't
// match it again
links.get(a).skip = true;
return foundList;
}
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch5p1_GraphNode from, ch5p1_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return true;
}
}
return false;
}// end match()
}// end Graph.java
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch5p1_Link {
ch5p1_GraphNode from;
ch5p1_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch5p1_Link(ch5p1_GraphNode from, ch5p1_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
在上面的程序中，dfsTraverse()方法应用了一种递归方法来执行深度优先搜索，其中使用了一种match()方法来检查线路图并确定在 from 和 to 之间是否有任何路径。如果答案是肯定的，那么它获得信息并将路由连接信息推入栈，然后返回路由路径。否则，如果答案是否定的，它就调用一个find(方法来检查 from 和其他城市之间的路径，如果可以找到，程序就返回连接信息的对象，否则程序返回 null。如果它能找到这样的路径，那么它可以将信息推到栈顶，然后递归地调用dfsTraverse()，并将城市保存到新的出发城市中。否则，程序返回并递归调用dfsTraverse()，直到找到目的地。
运行该程序的结果如下:
The path from your current city to the destination city is:
A->B->F->B->M->N->M->O->M->B->G->B->A->C->H->C->I->P->S
这个结果显示了使用 DFS 搜索策略遍历图时的确切路径，包括更详细的回溯。
基于莱霍斯·EV3 的 DFS 算法
在基于 leJOS 的 DFS 算法中，路径上的每个节点都是一个名为 WPNode 的类节点，定义如下:
public WPNode(String newname, WayPoint newwp) {
nodename = newname;
nodewp = newwp;
seen = false;
parent = this;
connections = new ArrayList<WPNode>();
}
基于 leJOS 的 DFS 算法的伪代码描述如下:
-
构建搜索空间的类属树，如:
A = new WPNode("A", new WayPoint(0, 0)); B = new WPNode("B", new WayPoint(-5, 5)); C = new WPNode("C", new WayPoint(5, 5)); A.addLeaf(B); A.addLeaf(C); -
声明一个栈来保存路由路径，例如:
Stack<WPNode> DFSpath = new Stack<WPNode>(); -
将当前节点设置为根节点，比如 a。如果找不到目标节点，则循环以下内容:
- 如果当前节点有子节点，则将第一个不可见的节点设置为当前节点，然后返回。
- 如果当前节点没有看不见的子节点，则将其父节点设置为当前节点，然后返回。
-
找到目的节点后，将目的节点推入栈，然后将每个父节点推入栈。
-
为双马达运动生成引导，然后设置引导以使用适当的尺寸和马达。
-
弹出每个路径节点的航路点，应用
goto(int x, int y)方法指挥机器人移动到下一个节点。
基于上述基于 leJOS 的 DFS 算法，你可以为你的机器人开发一个程序，使它能够在起始节点 A 和目的节点 M 之间的路径上行进，如图 5-3 所示:
- A 处的坐标是(0，0)。
- B 点的坐标是(-5，5)。
- C 点的坐标是(5，5)。
- D 点的坐标是(-10，10)。
- E 点的坐标是(0，10)。
- F 点的坐标是(-5，15)。
- 在 G 点的坐标是(5，15)。
- H 处的坐标是(-10，20)。
- I 处的坐标是(0，20)。
- J 点的坐标是(-15，25)。
- K 处的坐标是(-5，25)。
- L 处的坐标是(-10，30)。
- M 处的坐标为(0，30)。
图 5-3。
Path in between two nodes
你的程序至少应该在 LCD 上显示目的地的坐标，然后显示消息“Press ENTER key to continue.”，按回车键，你的机器人移动到下一个节点。例如，假设你的机器人从A (0,0)出发，你想探索一条通往E (0,10)的道路。假设你的机器人使用深度优先搜索探索的路径是A -> B -> E。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
B(-5,5)，并显示信息Press ENTER key to continue。 - 转到坐标为
-5,5的位置。 - 在 LCD 上显示目的地的坐标
E(0, 10)，并显示信息Press ENTER key to continue. - 转到坐标为
0,10的位置。
而且你的问题应该有一个字符串叫 destination，所以在改变 destination 的值的时候足够智能，你的机器人可以探索一条从起始节点 A 到新的目的节点的新路径。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 DFS 算法的实现，以探索从节点 A 到目的节点 M 的路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_main.java
//Driver class to set up map using ch5p2_GraphNode, ch5p2_Link, and //ch5p2_Graph classes.
//Calls a depth-first search in ch5p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch5p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks // like
ch5p2_Graph searchGraph = new ch5p2_Graph();
ch5p2_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M;
ch5p2_Link AB, AC, BD, BE, EF, EG, FH, FI, HJ, HK, KL, KM;
// define each node
A = new ch5p2_GraphNode("A", 0, 0);
B = new ch5p2_GraphNode("B", -5, 5);
C = new ch5p2_GraphNode("C", 5, 5);
D = new ch5p2_GraphNode("D", -10, 10);
E = new ch5p2_GraphNode("E", 0, 10);
F = new ch5p2_GraphNode("F", -5, 15);
G = new ch5p2_GraphNode("G", 5, 15);
H = new ch5p2_GraphNode("H", -10, 20);
I = new ch5p2_GraphNode("I", 0, 20);
J = new ch5p2_GraphNode("J", -15, 25);
K = new ch5p2_GraphNode("K", -5, 25);
L = new ch5p2_GraphNode("L", -10, 30);
M = new ch5p2_GraphNode("M", 0, 30);
// define which GraphNodes are connected
AB = new ch5p2_Link(A, B);
AC = new ch5p2_Link(A, C);
BD = new ch5p2_Link(B, D);
BE = new ch5p2_Link(B, E);
EF = new ch5p2_Link(E, F);
EG = new ch5p2_Link(E, G);
FH = new ch5p2_Link(F, H);
FI = new ch5p2_Link(F, I);
HJ = new ch5p2_Link(H, J);
HK = new ch5p2_Link(H, K);
KL = new ch5p2_Link(K, L);
KM = new ch5p2_Link(K, M);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(BD);
searchGraph.addLink(BE);
searchGraph.addLink(EF);
searchGraph.addLink(EG);
searchGraph.addLink(FH);
searchGraph.addLink(FI);
searchGraph.addLink(HJ);
searchGraph.addLink(HK);
searchGraph.addLink(KL);
searchGraph.addLink(KM);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// run depth-first search to get from start to
// destination
searchGraph.dfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(M)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// dfsTraverse
for (int i = searchGraph.dfsPath.size() - 1; i >= 0; i--) {
// go to node
navbot.goTo(searchGraph.dfsPath.get(i).xLocation,
searchGraph.dfsPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location " + searchGraph.dfsPath.get(i).cityName
+ ", ", 0, 0);
LCD.drawString(searchGraph.dfsPath.get(i).xLocation + ", "
+ searchGraph.dfsPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
// ch5p2_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************
public class ch5p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch5p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch5p2_Link {
ch5p2_GraphNode from;
ch5p2_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch5p2_Link(ch5p2_GraphNode from, ch5p2_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a depth-first
//search of the graph.
//the method dfsTraverse creates a dfsPath list which the robot will //follow to demonstrate
//how the depth-first search works.
//******************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch5p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch5p2_GraphNode> nodes;
ArrayList<ch5p2_Link> links;
// Two lists used for traversing
ArrayList<ch5p2_GraphNode> dfsTraverse;
Stack<ch5p2_Link> travelStack = new Stack<ch5p2_Link>();
// List used to define where you would like to move
ArrayList<ch5p2_GraphNode> dfsPath = new ArrayList<ch5p2_GraphNode>();
// Constructor of ch5p2_Graph class
public ch5p2_Graph() {
nodes = new ArrayList<ch5p2_GraphNode>();
links = new ArrayList<ch5p2_Link>();
dfsTraverse = new ArrayList<ch5p2_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch5p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch5p2_Link link) {
links.add(link);
}// end addLink
// dfsTraverse()
// perform depth-first search on the graph
public void dfsTraverse(ch5p2_GraphNode from, ch5p2_GraphNode to) {
boolean matched;
ch5p2_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to dfsPath
// This will be ultimately repeated by the end of the
// search
matched = match(from, to);
if (matched) {
travelStack.push(new ch5p2_Link(from, to));
dfsPath.add(new ch5p2_GraphNode(to.cityName,to.xLocation, to.yLocation));
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to dfsPath
// recursively call dfsTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch5p2_Link(from, to));
dfsTraverse(found.to, to);
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
dfsTraverse(found.from, found.to);
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
}
}// end dfsTraverse()
// find() method is used to
// find the next link to try exploring
public ch5p2_Link find(ch5p2_GraphNode from) {
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
// link found
if (links.get(a).from.equals(from) && !links.get(a).skip) {
ch5p2_Link foundList = new ch5p2_Link(links.get(a).from,
links.get(a).to);
// mark this link as used so we don't match it again
links.get(a).skip = true;
return foundList;
}
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch5p2_GraphNode from, ch5p2_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return true;
}
}
return false;
}// end match()
}// end Graph.java
摘要
在这一章中，你学习了深度优先搜索算法的基本原理，现在知道如何在实践中应用它来解决搜索程序。您还学习了如何基于深度优先搜索算法和导航类构建问题解决代理(即您的机器人)，您在前面的章节中学习了这些问题解决代理能够智能地找到从起点到任何目的地的路线路径。
在下一章中，你将学习如何在 leJOS EV3 上实现广度优先搜索(BFS)算法，以及如何将已实现的 BFS 算法集成到基于 leJOS 的机器人系统中进行定位和路径规划。
六、广度优先搜索及其在 Lego Mindstorms 中的实现
正如你在第五章中被介绍到深度优先搜索(DFS)算法一样，在这一章你将学习如何在莱霍斯 EV3 实现广度优先搜索(BFS)算法。具体而言，本章将涵盖以下主题:
- 一种新的广度优先搜索(BFS)算法，可以应用于建立任意树结构一般。
- 在基于 leJOS 的机器人系统中应用和集成所提出的 BFS 算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
BFS 算法综述
我们先来回顾一下人类是如何解决一个搜索问题的。首先，你需要一个表示你的搜索问题是如何存在的。图 6-1 是你的搜索树的一个例子。它显示了一系列相互连接的节点，您可以通过这些节点进行搜索:
图 6-1。
Tree structure of a path
广度优先搜索算法的工作原理是取一个节点；检查它的邻居；扩展它在其邻居中找到的第一个节点；检查该扩展节点是否是其目的地；如果没有，则继续探索其他相邻节点。例如，如果你想找到一条从 A 到 E 的路径，你可以使用两个列表来跟踪你在做什么:一个开放列表和一个封闭列表。开放列表记录你需要做什么，封闭列表记录你已经做了什么。
开始的时候，你只有你的起点，节点 a，你还没对它做什么，那就把它加入你的开放列表吧。现在你有了一个包含<A>的开放列表和一个包含<empty>的封闭列表。
现在让我们探索一下你的 A 节点的邻居。节点 A 的邻居是 B、C 和 D 节点。因为您现在已经完成了 A 节点，所以您可以将它从开放列表中移除，并将其添加到封闭列表中。那么当前开放列表包含<B, C, D>,，封闭列表包含<A>。现在，您的开放列表包含三个项目。
对于广度优先搜索，您总是从开放列表中探索第一个节点。开放列表中的第一项是 B 节点。b 不是你的目的地，现在我们来探索一下它的邻居。因为您现在已经扩展了 B，所以您将把它从开放列表中移除，并将其添加到封闭列表中。您的新节点是 E、F 和 G，您可以将这些节点添加到开放列表的末尾。然后你有一个包含<A,B>的封闭列表和一个包含< C, D, E, F, G>的开放列表。
接下来，展开 C 节点。由于它不是预期的目的地，您可以将其从开放列表中删除，并将其添加到封闭列表中。当前开放列表包含<D, E, F, G>,，而封闭列表包含<A, B, C>。c 没有子节点，所以您继续访问节点 d。因为它也不是预期的目的地，所以您可以将其从开放列表中删除，并将其添加到封闭列表中。因此，当前开放列表包含<E, F, G>，而 and 关闭列表包含<A, B, C, D>。
接下来展开节点 e。因为这是您的预期目的地，所以您停下来。因此，您会收到通过使用常规的广度优先搜索算法从封闭列表中解释的路线A->B->E。
接下来，让我们看看如何使用广度优先搜索方法来解决路径查找问题，该问题可以应用于从起始城市导航到目的城市的 GPS 系统。假设您想从城市 A(例如，新罕布什尔州的基恩)开车到城市 S(例如，马萨诸塞州的波士顿)。给定以下路线，使用广度优先搜索策略，为你制定一个从 A 出发到达 S 的计划。
| 城市 | 距离 | | --- | --- | | 从 a 到 B | 20 英里 | | 从 a 到 C | 10 英里 | | 从 a 到 D | 10 英里 | | 从 a 到 E | 20 英里 | | （back to back）背对背 | 10 英里 | | b 到 M | 20 英里 | | b 到 G | 10 英里 | | c 到 H | 10 英里 | | c 到 I | 15 英里 | | 从 d 到 J | 20 英里 | | e 到 K | 15 英里 | | e 到 L | 15 英里 | | m 到 N | 20 英里 | | 摩托 g | 20 英里 | | 从 I 到 P | 40 英里 | | 个人对个人 | 20 英里 | | 个人对个人 | 20 英里 |基于以上信息，你可以绘制一个路径树，如图 6-2 所示，显示两个城市之间所有可能的路径:
图 6-2。
Routes in between two cities
以下程序可用于通过使用广度优先搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p1_main.java
//Driver class to call a breadth-first search algorithm
//to create navigation path from a start to an end node.
//******************************************************************
import java.util.*;
class ch6p1_main {
// This array holds the connection information between two cities.
ch6p1_GraphNode cityLinks[] = new ch6p1_GraphNode[200];
// number of path connections on the route graph
int numConnections = 0;
// backtrack stack to store the city nodes
Stack closedList = new Stack();
public static void main(String args[]) {
// define the start city name
String from = "A";
// define the destination city name
String to = "S";
ch6p1_main bfsTraveral = new ch6p1_main();
// set up the route graph
bfsTraveral.graph();
bfsTraveral.isNode(from, to);
if (bfsTraveral.closedList.size() != 0) {
System.out
.println("the path from your current city to the destination city is: ");
bfsTraveral.BFSroute(to);
}
}
// Add link between two cities
void addLink(String parent, String child) {
if (numConnections < 200) {
cityLinks[numConnections] = new ch6p1_GraphNode(parent, child);
numConnections++;
} else
System.out.println("error to add link\n");
}
// Initialize the path link to construct the graph.
void graph() {
addLink("A", "B");
addLink("A", "C");
addLink("A", "D");
addLink("A", "E");
addLink("B", "F");
addLink("B", "M");
addLink("B", "G");
addLink("C", "H");
addLink("C", "I");
addLink("D", "J");
addLink("E", "K");
addLink("E", "L");
addLink("M", "N");
addLink("M", "O");
addLink("I", "P");
addLink("P", "R");
addLink("P", "S");
}
// determine to see if there is a link matched between
// startCity and endCity
// if match found, return true, otherwise return false
int matched(String from, String to) {
for (int i = numConnections - 1; i > -1; i--) {
if (cityLinks[i].startCity.equals(from)
&& cityLinks[i].endCity.equals(to) && !cityLinks[i].visited) {
// set up visited to true to prevent re-visit
cityLinks[i].visited = true;
// match found
return 1;
}
}
// match not found
return 0;
}
// Given parent to find any child connected with this parent
ch6p1_GraphNode find(String parent) {
for (int i = 0; i < numConnections; i++) {
if (cityLinks[i].startCity.equals(parent) && !cityLinks[i].visited) {
ch6p1_GraphNode f = new ch6p1_GraphNode(cityLinks[i].startCity,
cityLinks[i].endCity);
// set up visited to true to prevent re-visit
cityLinks[i].visited = true;
// child (or leaf) returned
return f;
}
}
// if parent has no child return nothing
return null;
}
// using breadth-first search and determining if there is any // route existing
// in between startCity and endCity
void isNode(String from, String to) {
int directconn;
ch6p1_GraphNode citynode;
Stack resetList = new Stack();
// determine if there is any direct link between from and to
// if yes push the link of the two cities into closedList
// stack
directconn = matched(from, to);
if (directconn != 0) {
closedList.push(
new ch6p1_GraphNode(from, to));
return;
}
// find all the children cities connected with the
// specified parent node
while ((citynode = find(from)) != null) {
resetList.push(citynode);
// check further if there is any direct
// connection between the child
// and grandchild
if ((directconn = matched(citynode.endCity, to)) != 0) {
resetList.push(citynode.endCity);
closedList.push(
new ch6p1_GraphNode(from, citynode.endCity));
closedList.push(
new ch6p1_GraphNode(citynode.endCity, to));
return;
}
}
// reset the visited boolean to unvisited and do the
// breadth first
// search next
for (int i = resetList.size(); i != 0; i--)
resetSkip((ch6p1_GraphNode) resetList.pop());
// then try the next neighboring city nodes
citynode = find(from);
if (citynode != null) {
closedList.push(
new ch6p1_GraphNode(from, to));
isNode(citynode.endCity, to);
} else if (closedList.size() > 0) {
// trace back and try another link
citynode = (ch6p1_GraphNode) closedList.pop();
isNode(citynode.startCity, citynode.endCity);
}
}
// reset visited field of specified parent city node
void resetSkip(ch6p1_GraphNode citynode) {
for (int i = 0; i < numConnections; i++)
if (cityLinks[i].startCity.equals(citynode.startCity)
&& cityLinks[i].endCity.equals(citynode.endCity))
cityLinks[i].visited = false;
}
// Show the route obtained by the BFS algorithm
void BFSroute(String to) {
int num = closedList.size();
Stack reverseList = new Stack();
ch6p1_GraphNode citynode;
// Reverse the stack to show the path
for (int i = 0; i < num; i++)
reverseList.push(closedList.pop());
for (int i = 0; i < num; i++) {
citynode = (ch6p1_GraphNode) reverseList.pop();
System.out.print(citynode.startCity + " -> ");
}
System.out.println(to);
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
//ch6p1_GraphNode.java
//Represents name of a graph
//******************************************************************
public class ch6p1_GraphNode {
String startCity;
String endCity;
// to determine if the city has been visited or not
boolean visited;
ch6p1_GraphNode(String s, String d) {
startCity = s;
endCity = d;
visited = false;
}
}
给定startCity A 和endCity S，运行程序的结果如下:
The path from your current city to the destination city is this:
A -> C -> I -> P -> S
这个结果显示了使用 BFS 搜索策略遍历图时的准确路径，不包括中间回溯节点。
基于莱霍斯·EV3 的 BFS 算法
在基于 leJOS 的 BFS 算法中，路径上的每个节点都是一个名为WPNode的类节点，定义如下:
public WPNode(String newname, WayPoint newwp) {
nodename = newname;
nodewp = newwp;
seen = false;
parent = this;
connections = new ArrayList<WPNode>();
}
以下是基于 leJOS 的 BFS 算法的伪代码描述:
-
构建搜索空间的类属树，如:
A = new WPNode("A", new WayPoint(0, 0)); B = new WPNode("B", new WayPoint(-5, 5)); C = new WPNode("C", new WayPoint(5, 5)); A.addLeaf(B); A.addLeaf(C); -
声明一个栈来保存路由路径，比如:
Stack<WPNode> BFSpath = new Stack<WPNode>(); -
将当前节点设置为根节点，比如 a。如果找不到目标节点，则循环以下内容:
- 如果当前节点有子节点，则将第一个不可见的节点设置为当前节点，然后返回。
- 如果当前节点没有看不见的子节点，则将其父节点设置为当前节点，然后返回。
-
找到目的节点后，将目的节点推入栈，然后将每个父节点推入栈。
-
为双马达运动生成引导，然后设置引导以使用适当的尺寸和马达。
-
弹出每个路径节点的路点，并应用
goto(int x, int y)方法来引导机器人移动到下一个节点。
基于上述基于 leJOS 的 BFS 算法，您可以为您的机器人开发一个程序，使其能够在起始节点 A 和目的节点 M 之间的路径上行进，如图 6-3 所示，其中您有以下内容:
图 6-3。
Path in between two nodes
The coordinate at A is (0,0).
The coordinate at B is (-5,5).
The coordinate at C is (5, 5).
The coordinate at D is (-10,10).
The coordinate at E is (0,10).
The coordinate at F is (-5,15).
The coordinate at G is (5,15).
The coordinate at H is (-10,20).
The coordinate at I is (0,20).
The coordinate at J is (-15,25).
The coordinate at K is (-5,25).
The coordinate at L is (-10,30).
The coordinate at M is (0,30).
你的程序至少应该在 LCD 上显示目的地的坐标，然后显示一条信息“Press ENTER key to continue”按回车键，你的机器人移动到下一个节点。例如，假设你的机器人从A (0,0)出发，你想探索一条通往G (5,15)的道路。假设你的机器人使用广度优先搜索探索的路径是A -> B -> E -> G。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
B(-5, 5)，并显示信息Press ENTER key to continue - 转到坐标为
-5,5的位置。 - 在 LCD 上显示目的地的坐标
E(0, 10)，并显示信息Press ENTER key to continue - 转到坐标为
0,10的位置。 - 在 LCD 上显示目的地的坐标
G(5, 15)，并显示信息Press ENTER key to continue - 转到坐标为
5,15.的位置
此外，你的问题应该有一个名为destination的字符串，这样它就足够智能，当改变目的地的值时，你的机器人可以探索一条从起始节点 A 到新目的地节点的新路径。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 BFS 算法的实现，以探索从节点 A 到目的节点 M 的路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_main.java
//Driver class to set up map using ch6p2_GraphNode, ch6p2_Link, and //ch6p2_Graph classes.
//Calls a breadth-first search in ch6p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch6p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks like
ch6p2_Graph searchGraph = new ch6p2_Graph();
ch6p2_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M;
ch6p2_Link AB, AC, BD, BE, EF, EG, FH, FI, HJ, HK, KL, KM;
// define each node
A = new ch6p2_GraphNode("A", 0, 0);
B = new ch6p2_GraphNode("B", -5, 5);
C = new ch6p2_GraphNode("C", 5, 5);
D = new ch6p2_GraphNode("D", -10, 10);
E = new ch6p2_GraphNode("E", 0, 10);
F = new ch6p2_GraphNode("F", -5, 15);
G = new ch6p2_GraphNode("G", 5, 15);
H = new ch6p2_GraphNode("H", -10, 20);
I = new ch6p2_GraphNode("I", 0, 20);
J = new ch6p2_GraphNode("J", -15, 25);
K = new ch6p2_GraphNode("K", -5, 25);
L = new ch6p2_GraphNode("L", -10, 30);
M = new ch6p2_GraphNode("M", 0, 30);
// define which GraphNodes are connected
AB = new ch6p2_Link(A, B);
AC = new ch6p2_Link(A, C);
BD = new ch6p2_Link(B, D);
BE = new ch6p2_Link(B, E);
EF = new ch6p2_Link(E, F);
EG = new ch6p2_Link(E, G);
FH = new ch6p2_Link(F, H);
FI = new ch6p2_Link(F, I);
HJ = new ch6p2_Link(H, J);
HK = new ch6p2_Link(H, K);
KL = new ch6p2_Link(K, L);
KM = new ch6p2_Link(K, M);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(BD);
searchGraph.addLink(BE);
searchGraph.addLink(EF);
searchGraph.addLink(EG);
searchGraph.addLink(FH);
searchGraph.addLink(FI);
searchGraph.addLink(HJ);
searchGraph.addLink(HK);
searchGraph.addLink(KL);
searchGraph.addLink(KM);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// run breadth-first search to get from start to
// destination
searchGraph.bfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(M)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// bfsTraverse
for (int i = 0; i < searchGraph.bfsTraverse.size(); i++) {
// go to node
navbot.goTo(searchGraph.bfsPath.get(i).xLocation,
searchGraph.bfsPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location " + searchGraph.bfsPath.get(i).cityName
+ ", ", 0, 0);
LCD.drawString(searchGraph.bfsPath.get(i).xLocation + ", "
+ searchGraph.bfsPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is pressed
buttons.waitForAnyPress();
if (i == searchGraph.bfsTraverse.size() - 1) {
navbot.goTo(searchGraph.bfsTraverse.get(i).to.yLocation,
searchGraph.bfsTraverse.get(i).to.xLocation);
LCD.drawString("At location "
+ searchGraph.bfsTraverse.get(i).to.cityName, 0, 0);
// block the thread until a button is
// pressed
buttons.waitForAnyPress();
}
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch6p2_Link {
ch6p2_GraphNode from;
ch6p2_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch6p2_Link(ch6p2_GraphNode from, ch6p2_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 //ch6p2_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************************************
public class ch6p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch6p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a breadth-first
//search traversal of the graph.
//the method bfsTraverse creates a bfsPath list which the robot will //follow to demonstrate
//how the breadth-first search works.
//******************************************************************************************
import java.util.ArrayList;
public class ch6p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch6p2_GraphNode> nodes;
ArrayList<ch6p2_Link> links;
// a list used for traversing
ArrayList<ch6p2_Link> bfsTraverse;
// List used to define where you would like to move
ArrayList<ch6p2_GraphNode> bfsPath = new ArrayList<ch6p2_GraphNode>();
// Constructor of ch6p2_Graph class
public ch6p2_Graph() {
nodes = new ArrayList<ch6p2_GraphNode>();
links = new ArrayList<ch6p2_Link>();
bfsTraverse = new ArrayList<ch6p2_Link>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch6p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch6p2_Link link) {
links.add(link);
}// end addLink
// bfsTraverse()
// perform breadth-first search on the graph
public void bfsTraverse(ch6p2_GraphNode from, ch6p2_GraphNode to) {
ArrayList<ch6p2_Link> resetList = new ArrayList<ch6p2_Link>();
boolean matched = false;
ch6p2_Link l;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to bfsPath
// This will ultimately repeated by the end of the search
matched = match(from, to);
// add to bfsTraverse if match is found
if (matched) {
bfsTraverse.add(new ch6p2_Link(from, to));
return;
}
// continue while links exist
while ((l = find(from)) != null) {
resetList.add(l);
if ((matched = match(l.to, to))) {
resetList.add(new ch6p2_Link(l.from, to));
bfsTraverse.add(new ch6p2_Link(from, l.to));
bfsTraverse.add(new ch6p2_Link(l.to, to));
return;
}
}
for (int i = resetList.size(); i != 0; i--) {
resetSkip((ch6p2_Link) resetList.remove(i - 1));
}
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to bfsPath
// recursively call bfsTraverse with the link's to as // start and our
// destination as the end
l = find(from);
if (l != null) {
bfsTraverse.add(new ch6p2_Link(from, to));
bfsPath.add(new ch6p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
bfsTraverse(l.to, to);
}
// backtrack if you cannot find a new connection
else if (bfsTraverse.size() > 0) {
l = (ch6p2_Link) bfsTraverse.remove(bfsTraverse.size() - 1);
bfsPath.remove(bfsPath.size() - 1);
bfsTraverse(l.from, l.to);
}
}// end bfsTraverse
// resetSkip
// when backtracking reset skip flag so we can visit nodes
// again
public void resetSkip(ch6p2_Link l) {
for (int i = 0; i < links.size(); i++) {
if (links.get(i).from.equals(l.from)
&& links.get(i).to.equals(l.to)) {
links.get(i).skip = false;
}
}
}
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch6p2_GraphNode from, ch6p2_GraphNode to) {
// iterate through list of links
for (int x = links.size() - 1; x >= 0; x--) {
if (links.get(x).from.equals(from) && links.get(x).to.equals(to)
&& !links.get(x).skip) {
links.get(x).skip = true;
return true;
}
}
return false;
}// end match
// find() method is used to
// find the next link to try exploring
public ch6p2_Link find(ch6p2_GraphNode from) {
// iterate through the list of links
for (int x = 0; x < links.size(); x++) {
// link found
if (links.get(x).from.equals(from) && !links.get(x).skip) {
ch6p2_Link l = new ch6p2_Link(links.get(x).from,
links.get(x).to);
links.get(x).skip = true;
// mark this link as used so we don't
// match it again
return l;
}
}
return null; // not found
}// end find()
}// end Graph.java
摘要
在本章中，你学习了广度优先搜索算法的基本原理，以及如何在实践中应用它来解决搜索程序。您还学习了如何基于广度优先搜索算法和您在前面章节中学习的导航类来构建问题解决代理，在导航类中，问题解决代理智能地找到从起点到任何目的地的路线路径。
在下一章中，您将了解启发式搜索策略背后的基本思想，学习如何在 leJOS EV3 上实现爬山算法，然后将实现的爬山算法集成到基于 leJOS 的机器人系统中进行定位和路径规划。
扫一扫
621
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
