Created
June 19, 2017 10:09
Star
You must be signed in to star a gist
EMC snippets
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "pledge.h" | |
#include "config.h" | |
#include "math.h" | |
#include "taskmanager.h" | |
Pledge checkPledge(int direction, bool space_left, bool space_top, bool space_right) | |
{ | |
state_t state; | |
// Preferred direction | |
if (direction == 0) { | |
if (space_top) { | |
state = straight_pref; // Keep going forward | |
std::cout << "Dir: " << direction << ", Go forward in preferred direction" << std::endl; | |
} | |
else if (!space_top) { | |
state = turn_left; // Turn left when obstacle in front | |
direction--; | |
std::cout << "Dir: " << direction << ", Obstacle ahead, turn left" << std::endl; | |
} | |
else if (!space_left && !space_top && !space_right) { | |
state = turn_180; // Dead end detected | |
direction--; | |
direction--; | |
std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg" << std::endl; | |
} | |
} | |
// Other than preferred direction | |
else { | |
if (space_right) { | |
state = turn_right; // Always turn right when there is space | |
direction++; | |
std::cout << "Dir: " << direction << ", Turn right, unpreferred direction" << std::endl; | |
} | |
else if (!space_right && space_top) { | |
state = straight; | |
std::cout << "Dir: " << direction << ", Go straight, unpreferred direction" << std::endl; | |
} | |
else if (!space_right && !space_top && space_left) { | |
state = turn_left; | |
direction--; | |
std::cout << "Dir: " << direction << ", Go left, unpreferred direction" << std::endl; | |
} | |
else if (!space_right && !space_top && !space_left) { | |
state = turn_180; | |
direction--; | |
direction--; | |
std::cout << "Dir: " << direction << ", Dead end detected, rotate 180 deg, unpreferred direction" << std::endl; | |
} | |
} | |
return Pledge(direction, state); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "spacerecognition.h" | |
#include "config.h" | |
#include "math.h" | |
Spaces getSpace(emc::LaserData scan) | |
{ | |
bool space_right = true, space_top = true, space_left = true; | |
double beam_width = M_PI*BEAM_WIDTH_DEG/180; | |
double a; | |
for (unsigned int i = 0; i < scan.ranges.size(); i++){ | |
a = scan.angle_min + i * scan.angle_increment; // Current angle | |
if ((a < -M_PI/2 + beam_width/2) && (a > -M_PI/2 - beam_width/2)){ | |
// Right of robot, check for obstacles nearby | |
if ((scan.ranges[i] < DIST_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) { | |
space_right = false; | |
} | |
} else if ((a < beam_width/2) && (a > -beam_width/2)) { | |
// Top of robot, check for obstacles nearby | |
if ((scan.ranges[i] < DIST_FORWARD_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) { | |
space_top = false; | |
} | |
} else if ((a < M_PI/2 + beam_width/2) && (a > M_PI/2 - beam_width/2)) { | |
// Left of robot, check for obstacles nearby | |
if ((scan.ranges[i] < DIST_TRES_MAX) && (scan.ranges[i] > DIST_TRES_MIN)) { | |
space_left = false; | |
} | |
} | |
} | |
return Spaces(space_left, space_top, space_right); | |
} | |
bool getRightSpace(emc::LaserData scan, double dist_max) | |
{ | |
bool space_right = true; | |
double beam_width = M_PI*BEAM_WIDTH_DEG/180; | |
double a; | |
for (unsigned int i = 0; i < scan.ranges.size(); i++){ | |
a = scan.angle_min + i * scan.angle_increment; // Current angle | |
if ((a < -M_PI/2 + beam_width/2) && (a > -M_PI/2 - beam_width/2)){ | |
// Right of robot, check for obstacles nearby | |
if ((scan.ranges[i] < dist_max) && (scan.ranges[i] > DIST_TRES_MIN)) { | |
space_right = false; | |
} | |
} | |
} | |
return space_right; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "worldmodel.h" | |
Location Node::getLocation(){ | |
return location_; | |
} | |
int Node::getType(){ | |
return type_; | |
} | |
int Node::getID(){ | |
return ID_; | |
} | |
int Node::getLRFindex(){ | |
return LRFindex_; | |
} | |
void Node::setID(int node_ID){ | |
ID_ = node_ID; | |
} | |
void Node::setType(int node_type){ | |
type_ = node_type; | |
} | |
void Node::setLocation(Location node_location){ | |
location_ = node_location; | |
} | |
void Node::setLRFindex(int index){ | |
LRFindex_ = index; | |
} | |
void Node::addConnections(int new_ID){ | |
connections_.push_back(new_ID); | |
} | |
void Node::printInfo(){ | |
std::cout << "Node Info: " << std::endl; | |
std::cout << "\tID: " << ID_ << "\n\tType: " << type_ << "\n\tLocation(angle, distance): (" << location_.angle; | |
std::cout << " ," << location_.dist << ")" << std::endl; | |
std::cout << "\tConnection to other nodes (ID):" << connections_.size() << std::endl; | |
for(int connum = 0; connum < connections_.size(); connum++){ | |
std::cout << "\t" << connections_[connum] << std::endl; | |
} | |
} | |
// Evaluate each datapoint in the LRF data to indentify nodes | |
std::vector<Node> ObjectRecognition::scanNodes(emc::LaserData scan){ | |
int new_node_ID = 0; | |
int nodeID = 1; | |
int new_node_type = ENDNODE; | |
int scan_range_length = scan.ranges.size(); | |
bool node_recognized = false; | |
Location new_node_loc(0.0, 0.0); | |
Node new_node(new_node_ID, new_node_type, new_node_loc); // Current node considered | |
Node prev_node(new_node_ID, new_node_type, new_node_loc); // Previous node entered into the new_node_vector | |
std::vector<Node> new_node_vector; | |
// First and last node are endpoint per definition | |
new_node_type = ENDNODE; | |
// First data point | |
new_node_loc.setDist(scan.ranges[0]); | |
new_node_loc.setAngle(scan.angle_min); | |
new_node.setLocation(new_node_loc); | |
new_node.setID(0); | |
new_node.setLRFindex(0); | |
new_node.setType(new_node_type); | |
new_node_vector.push_back(new_node); | |
// The other points need to be checked | |
for (int i = 1; i < scan_range_length - 1; i++){ | |
node_recognized = false; | |
// Test if datapoint is end node or corner node and add to node list | |
if (endNodeTest(scan, i)){ | |
new_node_type = ENDNODE;; | |
node_recognized = true; | |
} | |
else if (cornerNodeTest(scan, i, CORNERNODELOOP) || cornerEndNodeTest(scan, i)){ | |
new_node_type = CORNERNODE; | |
node_recognized = true; | |
} | |
if (node_recognized){ | |
// Set node location | |
new_node_loc.setAngle(scan.angle_min + scan.angle_increment*i); | |
new_node_loc.setDist(scan.ranges[i]); | |
new_node_ID = nodeID; | |
// Set new node and add it to the new node vector | |
new_node.setID(new_node_ID); | |
new_node.setLRFindex(i); | |
new_node.setType(new_node_type); | |
new_node.setLocation(new_node_loc); | |
// Check if nodes are to close. | |
// If true do not set new node. | |
if (proximityTest(new_node, prev_node) == false){ | |
new_node_vector.push_back(new_node); | |
// Now check if connections need to be added to nodes | |
// std::cout << "Node ID: " << new_node.getID() << std::endl; | |
if (checkPrevNodeCon(new_node_vector)){ | |
addPrevNodeCon(new_node_vector); | |
} | |
if (new_node.getType() == ENDNODE){ | |
nodeID++; | |
} | |
else if (new_node.getType() == CORNERNODE){ | |
nodeID++; | |
} | |
prev_node = new_node; | |
} | |
} | |
} | |
// Last node at last data point | |
new_node_ID = nodeID; | |
new_node_type = ENDNODE; | |
new_node_loc.setAngle(scan.angle_min + scan.angle_increment*(scan.ranges.size()-1)); | |
new_node_loc.setDist(scan.ranges[scan.ranges.size()-1]); | |
new_node.setLocation(new_node_loc); | |
new_node.setID(nodeID); | |
new_node.setLRFindex(scan_range_length - 1); | |
new_node.setType(new_node_type); | |
new_node_vector.push_back(new_node); | |
nodeID++; | |
// Now check if connections need to be set | |
if(checkPrevNodeCon(new_node_vector)){ | |
addPrevNodeCon(new_node_vector); | |
} | |
return new_node_vector; | |
} | |
bool ObjectRecognition::endNodeTest(emc::LaserData scan, int index){ | |
double prev_dist = scan.ranges[index-1]; // Distance of previous datapoint | |
double cur_dist = scan.ranges[index]; // Distance of current datapoint | |
double next_dist = scan.ranges[index+1]; // Distance of next datapoint | |
if ((cur_dist-next_dist) > ENDNODENORM || cur_dist-prev_dist > ENDNODENORM || (cur_dist-scan.range_max<MAXRANGENORM && cur_dist-scan.range_max>-MAXRANGENORM)){ | |
return true; | |
} | |
else{ | |
return false; | |
} | |
return true; | |
} | |
bool ObjectRecognition::cornerNodeTest(emc::LaserData scan, int index, int nloop){ | |
int index_count = MINCORNERNODEINDEX; // Counter for number of indexes offset of data point checked | |
int scan_length = scan.ranges.size(); // Number of data samples of LRF | |
int max_index = 0; | |
int min_index = 0; | |
bool max_index_checked = false; | |
bool min_index_checked = false; | |
// Location of datapoints | |
double prev_dist, cur_dist, next_dist, prev_angle, cur_angle, next_angle, xpos_cur, ypos_cur, xpos_next, ypos_next, xpos_prev, ypos_prev, delta_r; | |
double a, b, c; | |
bool testres = false; | |
// Determine maximum index for the corner node check | |
// Checks radius and stops looping if no more data is available | |
max_index = index_count; | |
while((delta_r < MAXCONRNERRADIUS) && index+index_count < scan_length){ | |
cur_dist = scan.ranges[index]; | |
cur_angle = scan.angle_min+scan.angle_increment*(index); | |
next_dist = scan.ranges[index+index_count]; | |
next_angle = scan.angle_min+scan.angle_increment*(index+index_count); | |
// Position of current data point | |
xpos_cur = cos(cur_angle)*cur_dist; | |
ypos_cur = sin(cur_angle)*cur_dist; | |
// Position of next data point | |
xpos_next = cos(next_angle)*next_dist; | |
ypos_next = sin(next_angle)*next_dist; | |
delta_r = (pow(xpos_cur - xpos_next,2)+pow(ypos_cur - ypos_next,2)); | |
max_index = index_count; | |
max_index_checked = true; | |
index_count++; | |
} | |
// And the minimum index | |
index_count = MINCORNERNODEINDEX; | |
min_index = index_count; | |
delta_r = 0; | |
while (delta_r < MAXCONRNERRADIUS && index-index_count > 0){ | |
cur_dist = scan.ranges[index]; | |
cur_angle = scan.angle_min+scan.angle_increment*(index); | |
prev_dist = scan.ranges[index-index_count]; | |
prev_angle = scan.angle_min+scan.angle_increment*(index-index_count); | |
// Position of current data point | |
xpos_cur = cos(cur_angle)*cur_dist; | |
ypos_cur = sin(cur_angle)*cur_dist; | |
// Position of next data point | |
xpos_prev = cos(prev_angle)*prev_dist; | |
ypos_prev = sin(prev_angle)*prev_dist; | |
delta_r = (pow(xpos_cur - xpos_prev,2)+pow(ypos_cur - ypos_prev,2)); | |
min_index = index_count; | |
min_index_checked = true; | |
index_count++; | |
} | |
// Check if both maximum and minimum index are set correctly, else skip setting the node | |
if (max_index_checked && min_index_checked){ | |
for(int i = 1; i <= nloop; i++){ | |
prev_dist = scan.ranges[index-min_index/i]; // Distance of previous datapoint | |
cur_dist = scan.ranges[index]; // Distance of current datapoint | |
next_dist = scan.ranges[index+max_index/i]; // Distance of next datapoint | |
prev_angle = scan.angle_min+scan.angle_increment*(index-min_index/i); | |
cur_angle = scan.angle_min+scan.angle_increment*(index); | |
next_angle = scan.angle_min+scan.angle_increment*(index+max_index/i); | |
a = sqrt(pow(prev_dist,2)+pow(cur_dist,2)-2*prev_dist*cur_dist*cos(cur_angle-prev_angle)); | |
b = sqrt(pow(next_dist,2)+pow(cur_dist,2)-2*next_dist*cur_dist*cos(next_angle-cur_angle)); | |
c = sqrt(pow(next_dist,2)+pow(prev_dist,2)-2*next_dist*prev_dist*cos(next_angle-prev_angle)); | |
if ((pow(a,2)+pow(b,2)-pow(c,2)) <= CORNERNODENORM && (pow(a,2)+pow(b,2)-pow(c,2)) >= -CORNERNODENORM){ | |
testres = true; | |
} | |
else{ | |
testres = false; | |
break; | |
} | |
} | |
} | |
return testres; | |
} | |
bool ObjectRecognition::cornerEndNodeTest(emc::LaserData scan, int index){ | |
bool testres = false; | |
double prev_dist; // Distance of previous datapoint | |
double cur_dist; // Distance of current datapoint | |
double next_dist; // Distance of next datapoint | |
prev_dist = scan.ranges[index-1]; // Distance of previous datapoint | |
cur_dist = scan.ranges[index]; // Distance of current datapoint | |
next_dist = scan.ranges[index+1]; // Distance of next datapoint | |
if ((cur_dist-next_dist) < -ENDNODENORM || cur_dist-prev_dist < -ENDNODENORM){ | |
testres = true; | |
} | |
return testres; | |
} | |
bool ObjectRecognition::proximityTest(Node new_node, Node prev_node){ | |
if ((pow(new_node.getLocation().xpos - prev_node.getLocation().xpos,2)+pow(new_node.getLocation().ypos - prev_node.getLocation().ypos,2)) <= pow(FILTERSIZE,2)){ | |
return true; | |
} | |
return false; | |
} | |
bool ObjectRecognition::doorCheck(std::vector<Node> node_vector, double coordx[], double coordy[]) | |
{ | |
int NNodes = node_vector.size(); //Number of nodes | |
int node1type, node2type, node3type, node4type; | |
bool res = false; | |
double node1x, node1y, node1r, node1a, node2x, node2y, node2r, node2a; | |
double node3x, node3y, node3r, node3a, node4x, node4y, node4r, node4a; | |
double dist12, dist23, dist34; | |
double a1, b1, c1, a2, b2, c2, a11, a22; | |
double L11, L12, L13, L14, L15, L16; | |
double L21, L22, L23, L24, L25, L26; | |
double point1side, point4side; | |
for(int i = 0; i < NNodes-3 ; i++){ | |
Node node1 = node_vector[i]; | |
node1x = node1.getLocation().xpos; | |
node1y = node1.getLocation().ypos; | |
Node node2 = node_vector[i+1]; | |
node2x = node2.getLocation().xpos; | |
node2y = node2.getLocation().ypos; | |
Node node3 = node_vector[i+2]; | |
node3x = node3.getLocation().xpos; | |
node3y = node3.getLocation().ypos; | |
Node node4 = node_vector[i+3]; | |
node4x = node4.getLocation().xpos; | |
node4y = node4.getLocation().ypos; | |
// Check if values are correct | |
dist12= sqrt(pow(node1x-node2x,2)+pow(node1y-node2y,2)); | |
dist23= sqrt(pow(node2x-node3x,2)+pow(node2y-node3y,2)); | |
dist34= sqrt(pow(node3x-node4x,2)+pow(node3y-node4y,2)); | |
if (dist12 <= DOOR_DIST+SIDEDOORCHECKMARGINSIDE && dist12 >= 0.3-SIDEDOORCHECKMARGINSIDE && dist34 <= DOOR_DIST+SIDEDOORCHECKMARGINSIDE && dist34 >= 0.3-SIDEDOORCHECKMARGINSIDE && dist23 <= 1.5+DOORCHECKMARGINBACK && dist23 >= 0.5-DOORCHECKMARGINBACK){ | |
// Check if both not-door-nodes are on the same side of the door | |
point1side = (node1x-node2x)*(node3y-node2y)-(node1y-node2y)*(node3x-node2x); | |
point4side = (node4x-node2x)*(node3y-node2y)-(node4y-node2y)*(node3x-node2x); | |
if ((point1side <= 0 && point4side <=0) || (point1side >= 0 && point4side >=0)){ | |
// Check if all nodes are the correct type | |
node1type = node1.getType(); | |
node2type = node2.getType(); | |
node3type = node3.getType(); | |
node4type = node4.getType(); | |
if((node1type == CORNERNODE || node1type == ENDNODE) && node2type == CORNERNODE && node3type == CORNERNODE && (node4type == CORNERNODE || node4type == ENDNODE)){ | |
// Check if the two middle nodes are around 90 degrees | |
node1r = node1.getLocation().dist; | |
node1a = node1.getLocation().angle; | |
node2r = node2.getLocation().dist; | |
node2a = node2.getLocation().angle; | |
node3r = node3.getLocation().dist; | |
node3a = node3.getLocation().angle; | |
node4r = node4.getLocation().dist; | |
node4a = node4.getLocation().angle; | |
// Values important to check if node3 is 90 degrees | |
a1 = sqrt(pow(node4r,2)+pow(node3r,2)-2*node4r*node3r*cos(node3a-node4a)); | |
b1 = sqrt(pow(node2r,2)+pow(node3r,2)-2*node2r*node3r*cos(node2a-node3a)); | |
c1 = sqrt(pow(node2r,2)+pow(node4r,2)-2*node2r*node4r*cos(node2a-node4a)); | |
// Values important to check if node2 is 90 degrees | |
a2 = sqrt(pow(node3r,2)+pow(node2r,2)-2*node3r*node2r*cos(node2a-node3a)); | |
b2 = sqrt(pow(node1r,2)+pow(node2r,2)-2*node1r*node2r*cos(node1a-node2a)); | |
c2 = sqrt(pow(node1r,2)+pow(node3r,2)-2*node1r*node3r*cos(node1a-node3a)); | |
if(((pow(a1,2)+pow(b1,2)-pow(c1,2)) <= SIDEDOORCORNERNODENORM && (pow(a1,2)+pow(b1,2)-pow(c1,2)) >= -SIDEDOORCORNERNODENORM) && ((pow(a2,2)+pow(b2,2)-pow(c2,2)) <= SIDEDOORCORNERNODENORM && (pow(a2,2)+pow(b2,2)-pow(c2,2)) >= -SIDEDOORCORNERNODENORM)){ | |
// Check if in area. These are four different checks. They state that you are always having one node A in front of you. If you are in the area, Node A+1 should lie on the left and A-1 should lie on the right. A+2 should lie behind you. | |
// Calculate the point "3" of the square | |
L11 = node3x-node4x; | |
L12 = node3y-node4y; | |
L13 = sqrt(pow(L11,2)+pow(L12,2)); | |
a11 = atan(L12/L11); | |
L14 = DOOR_DIST; | |
L15 = cos(a11)*L14; | |
L16 = sin(a11)*L14; | |
// Calculate the point "4" of the square | |
L21 = node2x-node1x; | |
L22 = node2y-node1y; | |
L23 = sqrt(pow(L21,2)+pow(L22,2)); | |
a22 = atan(L22/L21); | |
L24 = DOOR_DIST; | |
L25 = cos(a22)*L24; | |
L26 = sin(a22)*L24; | |
// Define the coordinates of the door area | |
coordx[0] = node2x; | |
coordy[0] = node2y; | |
coordx[1] = node3x; | |
coordy[1] = node3y; | |
coordx[2] = node3x-L15; | |
coordy[2] = node3y-L16; | |
coordx[3] = node2x-L25; | |
coordy[3] = node2y-L26; | |
if (coordx[0] > 0 && coordy[3] < 0 && coordx[2] < 0 && coordy[1] > 0){ | |
res = true; | |
} | |
else if (coordx[3] > 0 && coordy[2] < 0 && coordx[1] < 0 && coordy[0] > 0){ | |
res = true; | |
} | |
else if (coordx[2] > 0 && coordy[1] < 0 && coordx[0] < 0 && coordy[3] > 0){ | |
res = true; | |
} | |
else if (coordx[1] > 0 && coordy[0] < 0 && coordx[3] < 0 && coordy[2] > 0){ | |
res = true; | |
} | |
} | |
} | |
} | |
} | |
} | |
return res; | |
} | |
bool ObjectRecognition::checkPrevNodeCon(std::vector<Node> node_vector){ | |
// Check if node vector is large enough to check | |
if (node_vector.size() > 1){ | |
// Define index of previous and next node and check if there are datapoints inbetween | |
int cur_index = node_vector.size() - 1; | |
int prev_index = cur_index - 1; | |
int cur_LRF_index = node_vector[cur_index].getLRFindex(); | |
int prev_LRF_index = node_vector[prev_index].getLRFindex(); | |
int LRF_index_diff = cur_LRF_index - prev_LRF_index; | |
// If the LRF index difference is smaller than NODECONINDEXDIFF than the nodes are NOT connected. | |
if (LRF_index_diff > NODECONINDEXDIFF){ | |
return true; | |
}else{ | |
return false; | |
} | |
}else{ | |
return false; | |
} | |
} | |
void ObjectRecognition::addPrevNodeCon(std::vector<Node> &node_vector){ | |
// Check if node vector is large enough to check | |
if (node_vector.size() > 1){ | |
// Get pointer to nodes in vector | |
int cur_index = node_vector.size() - 1; | |
int prev_index = cur_index - 1; | |
int cur_id = node_vector[cur_index].getID(); | |
int prev_id = node_vector[prev_index].getID(); | |
// Set connection vector of previous and current node | |
node_vector[cur_index].addConnections(prev_id); | |
node_vector[prev_index].addConnections(cur_id); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment