前两天实验室买的机器人pioneer3-DX终于拿到手里了,抓紧时间在Fedora Core 6下测试了下。不试不知道,一试吓一跳,先是无法安装机器人仿真软件mobilesim,然后是编写的机器人控制程序无法获取机器人的位置信息。下面将解决的过程发表出来,以便以后再有人遇到类似的问题能够有所参照。
首先安装机器人仿真软件mobilesim。
由于机器人附件光盘里的mobilesim的rpm安装包需要libsft.so.1的支持,这个要求与Fedora Core 6系统的libsft.so.2相冲突,所以不直接使用该rpm包安装,必须使用源码安装。下载MobileSim的源代码
#wget http://robots.mobilerobots.com/MobileSimSource/download/MobileSim-src-0.4.0.tgz
解压:
#tgz. tar -xzvf MobileSim-src-0.4.0.tgz
#cd MobileSim-src-0.4.0
编译:
#make
安装:
#make install
然后在Fedora core 6的“应用程序-->其他”里出现Mobilesim
注意在使用Mobilesim进行仿真的时候必须让eth0处于激活状态,并且必须用网线连接到路由器,否则会出现无法打开TCPPORT的错误(反正我使用双机对连线连接到另一台机器来激活eth0也不能进行仿真)
然后安装pioneer3-DX的支持库ARIA
可以直接使用机器人附件光盘里的ARIA的rpm包进行安装,安装后就能够编写自己的机器人控制程序,编译和运行都不会报错。但是这样会出现机器人控制程序无法获取机器人位置信息的情况,因此使用机器人附件光盘里的ARIA的源码包进行安装。拷贝源码包到任意目录:
#cp ../ARIA/ARIA-2.5.1.tgz /home/p3robot
解压:
#cd /home/p3robot
#tar -xzvf ARIA-2.5.1.tgz
编译和安装
#cd ARIA-2.5.1
#make
#make install
#ldconfig
执行完上述步骤就会将ARIA安装的到/usr/local,到这里机器人pioneer3-DX的开发环境就安装完毕了。写个程序测试以下:
gotoActionExample.cpp代码如下(让机器人行走到坐标为(5000mm,5000mm)的地方):
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArPose myposition;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit/n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting/n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
gotoPoseAction.setGoal(ArPose(5000, 5000));
for(;;)
{
if(robot.isRunning())
{
robot.lock();
printf("Position( %f, %f)/n",robot.getX(), robot.getY());
robot.unlock();
}
}
// Robot disconnected, shut down
Aria::shutdown();
return 0;
}
编译这个文件的Makegile如下:
SOURCE=gotoActionExample.cpp
OBJECTS=$(SOURCE:%.c=%.o)
TARGET = Goto
OPTION=-Wall -g -pthread
INCLUDEPATH= -I/usr/local/Aria/include
LIBPATH=-L/usr/local/Aria/lib
LIBS=-lAria -lArNetworking -lAriaJava -lArNetworkingJava
CC=gcc
$(TARGET):$(OBJECTS)
$(CC) $(OPTION) $(INCLUDEPATH) $(LIBPATH) $(LIBS) -o $(TARGET) $(OBJECTS)
clean:
-rm -f $(TARGET) *.o *~
先编译make下生成可执行文件Goto
仿真测试下:
首先启动mobilesim,不加载map,可以看到mobilesim打开了一个TCP端口8101等待机器人控制程序的连接了
运行刚才编写的机器人控制程序Goto
#./Goto -rh 127.0.0.1 -remoteRobotTcpPort 8101
顺利的话就在mobilesim中可以看到机器人在动了,同时终端里也显示机器人的实时坐标。