#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/wait.h>
#include <math.h>
#include <strings.h>
#include <string.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include "high.h"
#include "mt-rand.h"
// The initial seed used for the random number generated can be set here.
#define SEED 1
// Default names for printing map files.
// File types are automatically appended.
#define MAP_PATH_NAME "map"
#define PARTICLES_PATH_NAME "particles"
//
// Globals
//
// The number of iterations between writing out the map for video. 0 is off.
int VIDEO = 10;
// The current commands being given to the robot for movement.
// Not used when the robot is reading data from a log file.
double RotationSpeed, TranslationSpeed;
// The means by which the slam thread can be told to halt, either by user command or by the end of a playback file.
int continueSlam;
int PLAYBACK_COMPLETE = 0;
//
//
// InitializeRobot
//
// Calls the routines in 'ThisRobot.c' to initialize the necessary hardware and software.
// Each call has an opportunity to return a value of -1, which indicates that the initialization of that
// part of the robot has failed. In that event, Initialize robot itself will return a general error of -1.
//
//
int InitializeRobot(int argc, char *argv[]) {
if (InitializeThisRobot(argc, argv) == -1) {
fprintf(stderr, "Start up initialization of the robot has failed.\n");
return -1;
}
fprintf(stderr, "Connect Odometry.\n");
if (ConnectOdometry(argc, argv) == -1) {
fprintf(stderr, "Unable to connect to the robot's odometry.\n");
return -1;
}
fprintf(stderr, "Connect Laser.\n");
if (ConnectLaser(argc, argv) == -1) {
fprintf(stderr, "Unable to connect to the robot's laser.\n");
return -1;
}
fprintf(stderr, "Connect Drive.\n");
if (ConnectDrive(argc, argv) == -1) {
fprintf(stderr, "Unable to connect to the robot's drive motors.\n");
return -1;
}
return 0;
}
//
// WriteLog
//
// Prints to file the data that we would normally be getting from sensors, such as the laser and the odometry.
// This allows us to later play back the exact run, with different parameters.
// All readings are in meters or radians.
//
void WriteLog(FILE *logFile, TSense sense)
{
int i;
fprintf(logFile, "Odometry %.6f %.6f %.6f \n", odometry.x, odometry.y, odometry.theta);
fprintf(logFile, "Laser %d ", SENSE_NUMBER);
for (i = 0; i < SENSE_NUMBER; i++)
fprintf(logFile, "%.6f ", sense[i].distance/MAP_SCALE);
fprintf(logFile, "\n");
}
//
// This calls the procedures in the other files which do all the real work.
// If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make
// certain to set LOW_DURATION in low.h to some incredibly high number.
//
void *Slam(void *a)
{
TPath *path, *trashPath;
TSenseLog *obs, *trashObs;
InitHighSlam();
InitLowSlam();
while (continueSlam) {
LowSlam(continueSlam, &path, &obs);
HighSlam(path, obs);
// Get rid of the path and log of observations
while (path != NULL) {
trashPath = path;
path = path->next;
free(trashPath);
}
while (obs != NULL) {
trashObs = obs;
obs = obs->next;
free(trashObs);
}
}
CloseLowSlam();
return NULL;
}
int main (int argc, char *argv[])
{
//char command[256], tempString[20];
int x;
//int y;
//double maxDist, tempDist, tempAngle;
int WANDER, EXPLORE, DIRECT_COMMAND;
pthread_t slam_thread;
RECORDING = "";
PLAYBACK = "";
for (x = 1; x < argc; x++) {
if (!strncmp(argv[x], "-R", 2))
RECORDING = "current.log";
if (!strncmp(argv[x], "-r", 2)) {
x++;
RECORDING = argv[x];
}
else if (!strncmp(argv[x], "-p", 2)) {
x++;
PLAYBACK = argv[x];
}
else if (!strncmp(argv[x], "-P", 2))
PLAYBACK = "current.log";
}
fprintf(stderr, "********** Localization Example *************\n");
if (PLAYBACK == "")
if (InitializeRobot(argc, argv) == -1)
return -1;
fprintf(stderr, "********** World Initialization ***********\n");
seedMT(SEED);
// Spawn off a seperate thread to do SLAM
//
// Should use semaphores or similar to prevent reading of the map
// during updates to the map.
//
continueSlam = 1;
pthread_create(&slam_thread, (pthread_attr_t *) NULL, Slam, &x);
fprintf(stderr, "*********** Main Loop (Movement) **********\n");
// This is the spot where code should be inserted to control the robot. You can go ahead and assume
// that the robot is localizing and mapping.
WANDER = 0;
EXPLORE = 0;
DIRECT_COMMAND = 0;
RotationSpeed = 0.0;
TranslationSpeed = 0.0;
pthread_join(slam_thread, NULL);
return 0;
}
