void Visualizer::Run(){
//std::cout << animationStatus << "...";
switch(animationStatus) {
case 0: clear(); break;
case 1: stroboscope(); break;
case 2: lightchange(); break;
case 3: fadeIn(); break;
case 4: pulseCircle(); break;
case 5: lines(); break;
case 6: circles(); break;
case 7: randomLines(); break;
case 8: square(); break;
case 9: randomPixel(); break;
case 10: rotateLine(); break;
}
return;
}
/**
* Combine the lines extracted from segments into a single set of lines which
* hopefully represent the walls in the maze. The first segment starts at 0,0
* and other segments need to be rotated to correspond to the relative rotation
* of the segment that precedes them, based on the turn made by the robot. Just
* rotating the points that correspond to the line start and end and then
* rotating around the point that ends the previous segment should work?
* Transform the point we are rotating around to the origin, rotate the points,
* then translate back.
*
* Should return the lines for the segments rotated and translated to the
* correct position relative to the map.
*
* Also processes the objects, rotating them to correspond with segment
* rotations. The objects are modified in place.
*/
std::vector<std::vector<Line> > SegmentStitching::processSegments(const std::vector<std::vector<Line> >& linesInSegments,
mapping_msgs::SegmentObjectVector& allSegmentObjects){
ROS_INFO("==================== Stitching segments ====================");
// Keep a list of the positions of the robot at the beginning and end of
// each segment, relative to the map, as opposed to each segment.
std::vector<pcl::PointXYZ> segmentPointChain;
//segmentPointChain.push_back(pcl::PointXYZ(0,0,0)); // chain starts at the origin
std::vector<std::vector<Line> > stitchedLines;
std::vector<mapping_msgs::ObjectVector> segmentObjects = allSegmentObjects.segmentObjects;
// update this point every loop to make it correspond to the start point of
// the segment in the global frame
pcl::PointXYZ segmentGlobalStart(0,0,0);
int xDir=0; // direction x
int yDir=1; // direction y
int rotation=0; // assume integer rotations to avoid float errors
/*float xEnd=0;
float yEnd=0;*/
for (size_t i = 0; i < linesInSegments.size(); i++) {
ROS_INFO("---------- Processing segment %d ----------", (int)i);
ROS_INFO_STREAM("Segment start point: " << segmentGlobalStart);
// End point of the segment is at the end of the list. Don't need to
// extract the first point because the values are all zero.
std::vector<mapping_msgs::SegmentPoint> segList = mapSegments[i].pointList;
mapping_msgs::SegmentPoint segmentEnd = segList.back();
// Extract the points from the segmentpoint. Start point for the segment
// should always be (0,0,0). The end point corresponds to the position
// of the robot at the end of the segment. We assume no motion in the x
// direction.
float odomDist = segmentEnd.odometry.distanceTotal;
pcl::PointXYZ segmentEndPoint(xDir * odomDist, yDir * odomDist, 0);
ROS_INFO_STREAM("Initial segment endpoint: " << segmentEndPoint);
// For segments after the first one, need to modify the xdir and ydir to
// correspond to the turn direction of the previous segment
if (i != 0){
int turnDirection = mapSegments[i-1].turnDirection;
if (turnDirection == mapping_msgs::MapSegment::LEFT_TURN){
ROS_INFO("Segment %d is a left turn from the previous segment (rotated 90)", (int)i);
if (1==xDir && 0==yDir){ // going along x axis -> switch to going along y
xDir=0;
yDir=1;
} else if (0==xDir && 1==yDir){ // going along y -> switch to going backwards on x
xDir=-1;
yDir=0;
} else if (-1==xDir && 0==yDir){ // going backwards along x -> switch to going backwards on y
xDir=0;
yDir=-1;
} else { // going backwards along y -> switch to going along x
xDir=1;
yDir=0;
}
rotation+=90;
} else if (turnDirection == mapping_msgs::MapSegment::RIGHT_TURN){
ROS_INFO("Segment %d is a right turn from the previous segment (rotated -90)", (int)i);
if (1==xDir && 0==yDir){
xDir=0;
yDir=-1;
} else if (0==xDir && -1==yDir){
xDir=-1;
yDir=0;
} else if (-1==xDir && 0==yDir){
xDir=0;
yDir=1;
} else {
xDir=1;
yDir=0;
}
rotation-=90;
} else if (turnDirection == mapping_msgs::MapSegment::U_TURN){
ROS_INFO("Segment %d is a u turn from the previous segment (rotated 180)", (int)i);
if (xDir!=0){
xDir=-xDir;
} else if (yDir!=0){
yDir=-yDir;
}
rotation+=180;
} else {
ROS_ERROR("Received map segment with invalid turn state.");
}
}
// Reset rotation to be in bounds [180, -180]
if (rotation > 180){
rotation = rotation - 360;
} else if (rotation < -180){
rotation = rotation + 360;
}
ROS_INFO("Rotation is now %d", rotation);
std::vector<Line> tmpLines;
// loop over lines in the segment
for(size_t j = 0; j < linesInSegments[i].size(); j++){
ROS_INFO("Processing line %d", (int)j);
Line tmpLine = linesInSegments[i][j];
ROS_INFO_STREAM("" << tmpLine);
// if the segment is rotated in the global frame, rotate each
// line accordingly
if (rotation != 0){
tmpLine = rotateLine(tmpLine,rotation);
}
// Translate both points on the line from their current position
// relative to the origin to the starting point of the line in
// the global frame.
ROS_INFO_STREAM("Translating line according to " << segmentGlobalStart);
tmpLine.start = tmpLine.start + segmentGlobalStart;
tmpLine.end = tmpLine.end + segmentGlobalStart;
ROS_INFO_STREAM("Translated line: " << tmpLine);
tmpLines.push_back(tmpLine);
}
stitchedLines.push_back(tmpLines);
// loop over objects in this segment and rotate and translate them.
for (size_t obj = 0; obj < segmentObjects[i].objects.size(); obj++) {
mapping_msgs::Object& found = segmentObjects[i].objects[obj];
// rotate the point around the origin
if (rotation != 0){
found.location = PCLUtil::rotatePointAroundOriginXY(found.location, rotation);
}
// translate it to its location in the rotated segment.
found.location.x += segmentGlobalStart.x;
found.location.y += segmentGlobalStart.y;
}
//Just add the difference and change the segmentEndPoint x and y
float newX = xDir * segmentEnd.odometry.distanceTotal + segmentGlobalStart.x;
float newY = yDir * segmentEnd.odometry.distanceTotal + segmentGlobalStart.y;
segmentEndPoint = pcl::PointXYZ(newX, newY, 0);
segmentPointChain.push_back(segmentEndPoint);
ROS_INFO("X direction: %d, Y direction: %d", xDir, yDir);
segmentGlobalStart = segmentEndPoint;
}
for (size_t i = 0; i < segmentPointChain.size(); i++) {
ROS_INFO_STREAM("Segment point " << i << ": " << segmentPointChain[i]);
}
return stitchedLines;
}
int mainLoop()
{
wchar_t ch = 0;
int running = 1;
int roundEnd = 0;
int i = 0;
while(running)
{
ch = getChar();
if(ch == 27)
break;
/*
* Rotates specified lines the
* specified amount (in radians
*/
rotateLine(line1,M_PI/1000);
rotateLine(line2,M_PI/900);
rotateLine(line3,-M_PI/900);
/*
* Firethruster handles input
*/
fireThruster(ch);
moveLander();
/*
* Time to sleep between frames
*/
usleep(1000);
if(roundEnd)
{
roundEnd = 0;
clearPrintScreen();
usleep(1000000);
if(fuel < 0)
running = 0;
}
/*
* Convert fuel int to string
* and add to textVector
*/
char fuelstr[7];
sprintf(fuelstr,"%d",fuel_int);
changeText(fuel,fuelstr);
char scorestr[7];
sprintf(scorestr,"%d",score_int);
changeText(score,scorestr);
draw();
switch(detectWin(platform1,platform2))
{
case 0:
break;
case 1:
printToScreen("Crashed!");
draw();
resetLander();
roundEnd = 1;
break;
case 21:
printToScreen("Landed too hard :( -100 fuel");
score_int += 50;
fuel_int -= 100;
draw();
resetLander();
roundEnd = 1;
break;
case 22:
printToScreen("Landed too hard :( -100 fuel");
score_int += 75;
fuel_int -= 100;
draw();
resetLander();
roundEnd = 1;
break;
case 31:
printToScreen("PERFECT LANDING!");
score_int += 300;
draw();
resetLander();
roundEnd = 1;
break;
case 32:
printToScreen("PERFECT LANDING!");
score_int += 200;
draw();
resetLander();
roundEnd = 1;
break;
case 4:
printToScreen("Out of fuel!");
draw();
resetLander();
roundEnd = 1;
}
}
exitVect();
return 0;
}
int main(void) {
_delay_ms(100);
// IO port initialisation
port_direction_init();
centre.x = ((float)LCD_WIDTH-1.0)/(float)2.0; //the X origin offset
centre.y = ((float)LCD_HEIGHT-1.0)/(float)2.0; //the Y origin offset
//set up the compass serial port
compass_init();
LCD_init();
SREG |= 0x80; //Set the global interrupt enable bit in the Status Register SREG, see section 6.3.1 of the datasheet.
LCD_clear();
//north
fixedPointLine northline1;
northline1.start.x = fip(centre.x);
northline1.start.y = fip(0);
northline1.end.x = fip(37);
northline1.end.y = fip(centre.y);
fixedPointLine northline2;
northline2.start.x = fip(centre.x);
northline2.start.y = fip(0);
northline2.end.x = fip(46);
northline2.end.y = fip(centre.y);
//north small
fixedPointLine northsmallline1;
northsmallline1.start.x = fip(41.5);
northsmallline1.start.y = fip(15);
northsmallline1.end.x = fip(46);
northsmallline1.end.y = fip(23.5);
fixedPointLine northsmallline2;
northsmallline2.start.x = fip(41.5);
northsmallline2.start.y = fip(15);
northsmallline2.end.x = fip(37);
northsmallline2.end.y = fip(23.5);
//horizontal line
fixedPointLine horizontalline;
horizontalline.start.x = fip(30);//37;
horizontalline.start.y = fip(centre.y);
horizontalline.end.x = fip(53);//46;
horizontalline.end.y = fip(centre.y);
//south
fixedPointLine southline1;
southline1.start.x = fip(centre.x);
southline1.start.y = fip(47);
southline1.end.x = fip(46);
southline1.end.y = fip(centre.y);
fixedPointLine southline2;
southline2.start.x = fip(centre.x);
southline2.start.y = fip(47);
southline2.end.x = fip(37);
southline2.end.y = fip(centre.y);
fixedPointPolygon northTriangle;
northTriangle.num_vertices = 4;
fixedPointCoord vertices[northTriangle.num_vertices];
vertices[0].x = fip(centre.x); //point
vertices[0].y = fip(0);
vertices[1].x = fip(46); //bottom right
vertices[1].y = fip(centre.y);
vertices[2].x = fip(centre.x); //middle arrow
vertices[2].y = fip(19);
vertices[3].x = fip(37); //bottom left
vertices[3].y = fip(centre.y);
northTriangle.vertices = vertices; //vertices points to the start of the array
fixedPointPolygon rotatedPoly;
fixedPointEdgeTable globalEdgeTable;
while (1) {
//TODO: use a timed interrupt to send the LCD update
//TODO: stop the LCD update during the compass update (MAYBE??)
//TODO: name public and private headers
//TODO: separate out LCD functions
updated = 0;
compass_transmit((char) 0x12);
while (!updated) {
}
LCD_clear_buff();
//north
//draw_line_l(rotateLine(northline1, angle));
//draw_line_l(rotateLine(northline2, angle));
//north small
/*draw_line_l(rotateLine(northsmallline1, angle));
draw_line_l(rotateLine(northsmallline2, angle));*/
//horizontal line
//draw_line_l(rotateLine(horizontalline, angle));
//south
draw_line_l(rotateLine(southline1, angle));
if (rotatePolygon(&northTriangle, angle, &rotatedPoly)) {
if (initialise_global_edge_table(&rotatedPoly, &globalEdgeTable)) {
//remember a logical check that after initialisation, there are at least 2 edges!
draw_polygon(&globalEdgeTable);
free(globalEdgeTable.edges);
globalEdgeTable.edges = NULL;
draw_line_l(rotateLine(southline2, angle));
}
free(rotatedPoly.vertices);
rotatedPoly.vertices = NULL;
}
//steps:
//rotate polygon - check
//http://www.cs.rit.edu/~icss571/filling/how_to.html
//initializing should return a bool for successful or not (cos of mallocing)
//initialize edges
//initializing global edge table (sorted) (size should be total number of edges)
//initializing parity
//initializing the scan-line
//initializing active edge table (size should be number of edges)
//fill polygon
//aim for a draw_scene function (pass an array of polygons and an angle)
// if N is 2 for a polygon, cast its array to a line
LCD_update();
}
}
