Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
EMC snippets
#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);
}
#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;
}
#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