Bug #866 - Add 'urbanski' MP10 robot and example liveries.

git-svn-id: https://svn.code.sf.net/p/speed-dreams/code/trunk@6439 30fe4595-0a0c-4342-8851-515496e4dcbd

Former-commit-id: 694ac36a4121d3370bdac1df9c6923c73b0ebd90
Former-commit-id: 73795f7935a9c065168397b6d3eb68a5b390a4a9

src/drivers/CMakeLists.txt

@@ -23,4 +23,5 @@ IF(NOT OPTION_OFFICIAL_ONLY)
   #SD_ADD_SUBDIRECTORY(hymie)
   #SD_ADD_SUBDIRECTORY(K1999)
 
+  SD_ADD_SUBDIRECTORY(urbanski)
 ENDIF()

src/drivers/urbanski/CMakeLists.txt

@@ -0,0 +1,11 @@
+INCLUDE(../../../cmake/macros.cmake)
+
+SET(ROBOT_NAME "urbanski")
+
+SET(ROBOT_SOURCES ${ROBOT_NAME}.cpp driver.cpp driver.h geoutil.cpp geoutil.h trackproc.cpp trackproc.h)
+
+ROBOT_MODULE(NAME ${ROBOT_NAME} VERSION 1.0.0 SOVERSION 1.0.0
+             INTERFACE LEGACY WELCOME
+             SOURCES ${ROBOT_SOURCES})
+
+# For data associated to the robot module, see data/drivers/urbanski/CMakeLists.txt

src/drivers/urbanski/driver.cpp

@@ -0,0 +1,179 @@
+#include "driver.h"
+
+#include <cmath>
+
+const double Driver::ACCEL_SPEED_MARGIN = 0.2;
+const double Driver::BRAKE_SPEED_MARGIN = 1.0;
+const double Driver::CUTTING_CORNER_MARGIN = 0.023;
+const double Driver::ENGINE_BRAKE_FACTOR = 0.5;
+const double Driver::EXCESSIVE_STEER_FACTOR = 0.85;
+const double Driver::FIRST_GEAR_SPEED = 5.0;
+const double Driver::GRAVITY = 9.81;
+const double Driver::INF_SPEED = 1000.0;
+const double Driver::SHIFT_RPM_MARGIN = 5.0;
+const double Driver::SKILL_LEVEL = 3;
+const double Driver::SLIP_SPEED_MARGIN = 2.0;
+const double Driver::SMOOTH_STEER_FACTOR = 0.65;
+const double Driver::SPEED_LIMIT_MULTIPLIER = 1.4;
+const double Driver::STEER_LOOK_AHEAD_DIST = 15.0;
+
+void Driver::drive(tSituation *s) {
+	updateState();
+	memset((void*) &car->ctrl, 0, sizeof(tCarCtrl));
+	car->_steerCmd = getSteerCmd(s);
+	car->_gearCmd = getGearCmd(s);
+	car->_accelCmd = getAccelCmd(s);
+	car->_brakeCmd = getBrakeCmd(s);
+}
+
+void Driver::endRace() {
+	trackProc.shutDown();
+}
+
+void Driver::setCar(tCarElt *car) {
+	this->car = car;
+	this->car->_skillLevel = SKILL_LEVEL;
+}
+
+void Driver::setTrack(tTrack *track, void **carParmHandle) {
+	trackProc.init(track);
+	*carParmHandle = NULL;
+}
+
+double Driver::getAccelCmd(tSituation *s) {
+	if (state == BRAKE || state == NO_THROTTLE)
+		return 0.0;
+	if (state == ACCELERATE)
+		return 1.0;
+	double r = car->_wheelRadius(REAR_RGT);
+	double ratio = car->_gearRatio[car->_gear + car->_gearOffset];
+	double maxRevs = car->_enginerpmRedLine;
+	double accel = curSpeedLimit / r * ratio / maxRevs;
+	if (car->_speed_x - ACCEL_SPEED_MARGIN > curSpeedLimit)
+		accel *= ENGINE_BRAKE_FACTOR;
+	return accel;
+}
+
+double Driver::getBrakeCmd(tSituation *s) {
+	if (state == BRAKE)
+		return 1.0;
+	return 0.0;
+}
+
+double Driver::getBrakingDistance(double vH, double vL, double friction) {
+	return (vH * vH - vL * vL) / (GRAVITY * friction);
+}
+
+int Driver::getGearCmd(tSituation *s) {
+	if (car->_gear <= 0 || car->_speed_x <= FIRST_GEAR_SPEED)
+		return 1;
+	if (shouldShiftUp())
+		return car->_gear + 1;
+	if (shouldShiftDown())
+		return car->_gear - 1;
+	return car->_gear;
+}
+
+double Driver::getSpeedLimit(tTrackSeg *seg) {
+	if (seg->type == TR_STR)
+		return INF_SPEED;
+	double friction = seg->surface->kFriction;
+	return SPEED_LIMIT_MULTIPLIER * std::sqrt(GRAVITY * friction * seg->radius);
+}
+
+double Driver::getSteerCmd(tSituation *s) {
+	Point target = getTargetPoint();
+	double angle = getAngle(carPoint, target);
+	angle -= car->_yaw;
+	NORM_PI_PI(angle);
+	double steerAngle = SMOOTH_STEER_FACTOR * angle / car->_steerLock;
+	if (std::abs(steerAngle) > EXCESSIVE_STEER_FACTOR && car->_speed_x > FIRST_GEAR_SPEED)
+		state = NO_THROTTLE;
+	return steerAngle;
+}
+
+Point Driver::getTargetPoint() {
+	Point target = getCenterOfSegmentEnd(car->_trkPos.seg->next);
+	double totalDistance = getDistanceToSegmentEnd(car);
+	for (tTrackSeg *seg = car->_trkPos.seg->next; totalDistance < STEER_LOOK_AHEAD_DIST; seg = seg->next) {
+		Point p = getTargetPointForSegment(seg);
+		if (isVisibleFromCurrentLocation(p, seg))
+			target = p;
+		totalDistance += seg->length;
+	}
+	return target;
+}
+
+Point Driver::getTargetPointForSegment(tTrackSeg *seg) {
+	return getCenterOfSegmentEnd(seg);
+}
+
+bool Driver::isSlipping() {
+	for (int i = 0; i < 4; i++)
+		if (car->_wheelSpinVel(i) * car->_wheelRadius(i) > car->_speed_x + SLIP_SPEED_MARGIN)
+			return true;
+	return false;
+}
+
+bool Driver::isVisibleFromCurrentLocation(Point & p, tTrackSeg *targetSeg) {
+	Vec v(carPoint, p);
+	v.norm();
+	for (tTrackSeg *seg = car->_trkPos.seg->next; seg != targetSeg; seg = seg->next) {
+		Vec sl(carPoint, Point(seg->vertex[TR_SL])), el(carPoint, Point(seg->vertex[TR_EL]));
+		Vec sr(carPoint, Point(seg->vertex[TR_SR])), er(carPoint, Point(seg->vertex[TR_ER]));
+		sl.norm(); el.norm();
+		sr.norm(); er.norm();
+		if (sl.cross(v) > -CUTTING_CORNER_MARGIN ||
+			el.cross(v) > -CUTTING_CORNER_MARGIN ||
+			sr.cross(v) <  CUTTING_CORNER_MARGIN ||
+			er.cross(v) <  CUTTING_CORNER_MARGIN)
+			return false;
+	}
+	return true;
+}
+
+bool Driver::shouldBrake() {
+	double speed = car->_speed_x;
+	if (speed - BRAKE_SPEED_MARGIN > curSpeedLimit)
+		return true;
+	double totalDistance = getDistanceToSegmentEnd(car);
+	double friction = car->_trkPos.seg->surface->kFriction;
+	double distanceToStop = getBrakingDistance(speed, 0.0, friction);
+	for (tTrackSeg *seg = car->_trkPos.seg->next; totalDistance < distanceToStop; seg = seg->next) {
+		double speedLimit = getSpeedLimit(seg);
+		double brakingDistance = getBrakingDistance(speed, speedLimit, seg->surface->kFriction);
+		if (speed - BRAKE_SPEED_MARGIN > speedLimit && totalDistance <= brakingDistance)
+			return true;
+		totalDistance += seg->length;
+	}
+	return false;
+}
+
+bool Driver::shouldMaintain() {
+	return car->_speed_x + ACCEL_SPEED_MARGIN >= curSpeedLimit;
+}
+
+bool Driver::shouldShiftDown() {
+	int gear = car->_gear;
+	int off = car->_gearOffset;
+	double d = car->_gearRatio[gear+off-1] / car->_gearRatio[gear+off];
+	bool isSafe = d * car->_enginerpm < car->_enginerpmMaxPw;
+	return gear > 2 && car->_enginerpm < car->_enginerpmMaxTq && isSafe;
+}
+
+bool Driver::shouldShiftUp() {
+	return car->_gear < car->_gearNb - 1 && car->_enginerpm >= car->_enginerpmRedLine - SHIFT_RPM_MARGIN;
+}
+
+void Driver::updateState() {
+	curSpeedLimit = getSpeedLimit(car->_trkPos.seg);
+	carPoint = Point(car->_pos_X, car->_pos_Y);
+	if (shouldBrake())
+		state = BRAKE;
+	else if (isSlipping() && car->_gear <= 1)
+		state = NO_THROTTLE;
+	else if (shouldMaintain())
+		state = MAINTAIN;
+	else
+		state = ACCELERATE;
+}

src/drivers/urbanski/driver.h

@@ -0,0 +1,61 @@
+#ifndef DRIVER_H
+#define DRIVER_H
+
+#include "geoutil.h"
+#include "trackproc.h"
+
+#include <car.h>
+#include <raceman.h>
+#include <robottools.h>
+#include <tgf.h>
+#include <track.h>
+
+class Driver {
+public:
+	void drive(tSituation *s);
+	void endRace();
+	void setCar(tCarElt *car);
+	void setTrack(tTrack *track, void **carParmHandle);
+
+private:
+	enum State { BRAKE, NO_THROTTLE, MAINTAIN, ACCELERATE };
+
+	double getAccelCmd(tSituation *s);
+	double getBrakeCmd(tSituation *s);
+	double getBrakingDistance(double vH, double vL, double friction);
+	int getGearCmd(tSituation *s);
+	double getSpeedLimit(tTrackSeg *seg);
+	double getSteerCmd(tSituation *s);
+	Point getTargetPoint();
+	Point getTargetPointForSegment(tTrackSeg *seg);
+	bool isSlipping();
+	bool isVisibleFromCurrentLocation(Point & p, tTrackSeg *targetSeg);
+	bool shouldBrake();
+	bool shouldMaintain();
+	bool shouldShiftDown();
+	bool shouldShiftUp();
+	void updateState();
+
+	tCarElt *car;
+	State state;
+	double curSpeedLimit;
+	Point carPoint;
+	TrackProc trackProc; //for future use
+
+	static const double ACCEL_SPEED_MARGIN;
+	static const double BRAKE_SPEED_MARGIN;
+	static const double CUTTING_CORNER_MARGIN;
+	static const double ENGINE_BRAKE_FACTOR;
+	static const double EXCESSIVE_STEER_FACTOR;
+	static const double FIRST_GEAR_SPEED;
+	static const double GRAVITY;
+	static const double INF_SPEED;
+	static const double SHIFT_RPM_MARGIN;
+	static const double SKILL_LEVEL;
+	static const double SLIP_SPEED_MARGIN;
+	static const double SMOOTH_STEER_FACTOR;
+	static const double SPEED_LIMIT_MULTIPLIER;
+	static const double STEER_LOOK_AHEAD_DIST;
+};
+
+#endif

src/drivers/urbanski/geoutil.cpp

@@ -0,0 +1,23 @@
+#include "geoutil.h"
+
+double getAngle(Point & a, Point & b) {
+	return atan2(b.y - a.y, b.x - a.x);
+}
+
+Point getCenterOfSegmentEnd(tTrackSeg *seg) {
+	return getWeightedPointAtSegmentEnd(seg, 1, 1);
+}
+
+double getDistanceToSegmentEnd(tCarElt *car) {
+	tTrackSeg *seg = car->_trkPos.seg;
+	if (seg->type == TR_STR)
+		return seg->length - car->_trkPos.toStart;
+	else
+		return (seg->arc - car->_trkPos.toStart) * seg->radius;
+}
+
+Point getWeightedPointAtSegmentEnd(tTrackSeg *seg, double wLeft, double wRight) {
+	double x = (wLeft * seg->vertex[TR_EL].x + wRight * seg->vertex[TR_ER].x) / (wLeft + wRight);
+	double y = (wLeft * seg->vertex[TR_EL].y + wRight * seg->vertex[TR_ER].y) / (wLeft + wRight);
+	return Point(x, y);
+}

src/drivers/urbanski/geoutil.h

@@ -0,0 +1,28 @@
+#ifndef GEOUTIL_H
+#define GEOUTIL_H
+
+#include <cmath>
+
+#include <car.h>
+#include <track.h>
+
+struct Point {
+	double x, y;
+	Point(double x = 0.0, double y = 0.0) : x(x), y(y) {}
+	Point(t3Dd p) : x(p.x), y(p.y) {}
+};
+
+struct Vec {
+	double x, y;
+	Vec(Point a, Point b) : x(b.x - a.x), y(b.y - a.y) {}
+	double cross(Vec & v) { return x * v.y - y * v.x; }
+	double len() { return std::sqrt(x * x + y * y); }
+	void norm() { x /= len(); y /= len(); }
+};
+
+double getAngle(Point & a, Point & b);
+Point getCenterOfSegmentEnd(tTrackSeg *seg);
+double getDistanceToSegmentEnd(tCarElt *car);
+Point getWeightedPointAtSegmentEnd(tTrackSeg *seg, double wLeft, double wRight);
+
+#endif

src/drivers/urbanski/trackproc.cpp

@@ -0,0 +1,58 @@
+#include "trackproc.h"
+
+void TrackProc::init(tTrack *track) {
+	distToNextCorner = new double[track->nseg];
+	distToPrevCorner = new double[track->nseg];
+	nextCornerType = new int[track->nseg];
+	prevCornerType = new int[track->nseg];
+	setCornerInfo(track);
+}
+
+double TrackProc::getDistToNextCorner(tTrackSeg *seg) {
+	return distToNextCorner[seg->id];
+}
+
+double TrackProc::getDistToPrevCorner(tTrackSeg *seg) {
+	return distToPrevCorner[seg->id];
+}
+
+int TrackProc::getNextCornerType(tTrackSeg *seg) {
+	return nextCornerType[seg->id];
+}
+
+int TrackProc::getPrevCornerType(tTrackSeg *seg) {
+	return prevCornerType[seg->id];
+}
+
+void TrackProc::shutDown() {
+	delete[] distToNextCorner;
+	delete[] distToPrevCorner;
+	delete[] nextCornerType;
+	delete[] prevCornerType;
+}
+
+tTrackSeg* TrackProc::nextSeg(tTrackSeg *seg, bool forwards) {
+	return forwards ? seg->next : seg->prev;
+}
+
+void TrackProc::setCornerInfo(tTrack *track) {
+	tTrackSeg *startSeg = track->seg;
+	while (startSeg->type == TR_STR)
+		startSeg = startSeg->next;
+	traverseTrack(startSeg, false, distToNextCorner, nextCornerType);
+	traverseTrack(startSeg, true, distToPrevCorner, prevCornerType);
+}
+
+void TrackProc::traverseTrack(tTrackSeg *startSeg, bool forwards, double *distToCorner, int *cornerType) {
+	double dist = 0.0;
+	int type = startSeg->type;
+	for (tTrackSeg *seg = nextSeg(startSeg, forwards); seg != startSeg; seg = nextSeg(seg, forwards)) {
+		if (seg->type != TR_STR) {
+			type = seg->type;
+			dist = 0.0;
+		}
+		distToCorner[seg->id] = dist;
+		cornerType[seg->id] = type;
+		dist += seg->length;
+	}
+}

src/drivers/urbanski/trackproc.h

@@ -0,0 +1,26 @@
+#ifndef TRACKPROC_H
+#define TRACKPROC_H
+
+#include <track.h>
+
+class TrackProc {
+public:
+	void init(tTrack *track);
+	double getDistToNextCorner(tTrackSeg *seg);
+	double getDistToPrevCorner(tTrackSeg *seg);
+	int getNextCornerType(tTrackSeg *seg);
+	int getPrevCornerType(tTrackSeg *seg);
+	void shutDown();
+
+private:
+	tTrackSeg* nextSeg(tTrackSeg *seg, bool forwards);
+	void setCornerInfo(tTrack *track);
+	void traverseTrack(tTrackSeg *startSeg, bool forwards, double *cornerDist, int *cornerType);
+
+	double *distToNextCorner;
+	int *nextCornerType;
+	double *distToPrevCorner;
+	int *prevCornerType;
+};
+
+#endif

src/drivers/urbanski/urbanski.cpp

@@ -0,0 +1,115 @@
+/***************************************************************************
+
+	file                 : urbanski.cpp
+	created              : Mon Nov 3 01:13:44 CET 2014
+	copyright            : (C) 2014 Patryk Urbanski
+
+ ***************************************************************************/
+
+#ifdef _WIN32
+#include <windows.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <tgf.h>
+#include <track.h>
+#include <car.h>
+#include <raceman.h>
+#include <robottools.h>
+#include <robot.h>
+
+#include "driver.h"
+
+static const int DRIVERS = 2;
+static Driver driver[DRIVERS];
+
+static void initTrack(int index, tTrack *track, void *carHandle, void **carParmHandle, tSituation *s);
+static void newrace(int index, tCarElt *car, tSituation *s);
+static void drive(int index, tCarElt *car, tSituation *s);
+static void endrace(int index, tCarElt *car, tSituation *s);
+static void shutdown(int index);
+static int  InitFuncPt(int index, void *pt);
+
+extern "C" int moduleInitialize(tModInfo *modInfo) {
+	memset(modInfo, 0, DRIVERS*sizeof(tModInfo));
+
+	for (int i = 0; i < DRIVERS; i++, modInfo++) {
+		modInfo->name    = "urbanski";		/* name of the module (short) */
+		modInfo->desc    = "";	/* description of the module (can be long) */
+		modInfo->fctInit = InitFuncPt;		/* init function */
+		modInfo->gfId    = ROB_IDENT;		/* supported framework version */
+		modInfo->index   = i;
+	}
+
+	GfOut("Initialized urbanski\n");
+	return 0;
+}
+
+extern "C" int moduleWelcome(const tModWelcomeIn* welcomeIn, tModWelcomeOut* welcomeOut) {
+	welcomeOut->maxNbItf = DRIVERS;
+	return 0;
+}
+
+extern "C" int moduleTerminate() {
+	GfOut("Terminated urbanski\n");
+	return 0;
+}
+
+/*
+ * Module entry point
+ */
+extern "C" int urbanski(tModInfo *modInfo) {
+	return moduleInitialize(modInfo);
+}
+
+/*
+ * Module exit point
+ */
+extern "C" int urbanskiShut() {
+	return moduleTerminate();
+}
+
+/* Module interface initialization. */
+static int InitFuncPt(int index, void *pt) {
+	tRobotItf *itf  = (tRobotItf *)pt;
+
+	itf->rbNewTrack = initTrack; /* Give the robot the track view called */
+				 /* for every track change or new race */
+	itf->rbNewRace  = newrace; 	 /* Start a new race */
+	itf->rbDrive    = drive;	 /* Drive during race */
+	itf->rbPitCmd   = NULL;
+	itf->rbEndRace  = endrace;	 /* End of the current race */
+	itf->rbShutdown = shutdown;	 /* Called before the module is unloaded */
+	itf->index      = index; 	 /* Index used if multiple interfaces */
+	driver[index] = Driver();
+	return 0;
+}
+
+/* Called for every track change or new race. */
+static void initTrack(int index, tTrack *track, void *carHandle, void **carParmHandle, tSituation *s) {
+	driver[index].setTrack(track, carParmHandle);
+}
+
+/* Start a new race. */
+static void newrace(int index, tCarElt *car, tSituation *s) {
+	driver[index].setCar(car);
+}
+
+/* Drive during race. */
+static void drive(int index, tCarElt *car, tSituation *s) {
+	driver[index].drive(s);
+}
+
+/* End of the current race */
+static void endrace(int index, tCarElt *car, tSituation *s) {
+	driver[index].endRace();
+}
+
+/* Called before the module is unloaded */
+static void shutdown(int index) {
+}
+