Ejemplo n.º 1
0
/*
Remove toy with input index from knownToys
*/
void removeToy(float index)
{
	int int_index = int(index);
	knownToys.erase(knownToys.begin()+int_index);
}
Ejemplo n.º 2
0
/*
Filters input coordinates
Called by subscriber to toy coordinates published by image processing
Calls update_toys when trust treshold for coordinate is reached
Obstacles are filtered out
Input:  input_coord->x: float x
        input_coord->y: float y
        input_coord->z: float toy_type (wooden == 1, small = 0)
*/
void checkInput(const geometry_msgs::Point::ConstPtr& input_coord) {	
    bool found = false;
    float dist = 9999; //initialise distance between new and old toy coordinates

    float x = input_coord->x;
    float y = input_coord->y;
	
    //Map area points for ignoring toy coordinates in obstacles:

    //Outside
    float map_x_begin   = -1.00;
	float map_x_end     = 2.63;
    float map_y_begin   = -1.62;
	float map_y_end     = 1.35;
    
    //Box
    float box_x_begin   = 0.44;
	float box_x_end     = 1.10;
	float box_y_begin   = 0.24;
	float box_y_end     = 0.80;
    
    //Pillar, currently not needed
    float pillar_x_begin   = 999.0;
	float pillar_x_end     = 999.0;
	float pillar_y_begin   = 999.0; 
	float pillar_y_end     = 999.0;
    
    //Middle wall
    float wall_x_begin   = 0.768;
	float wall_x_end     = 1.215; 
	float wall_y_begin   = -1.814;
	float wall_y_end     = -0.307;
    
    //White basket
    float white_x_begin   = -0.379;
	float white_x_end     = 0.004; 
	float white_y_begin   = -1.772;
	float white_y_end     = -1.539;
    
    //Blue basket, currently not needed
    float blue_x_begin   = 999.0;
	float blue_x_end     = 999.0;
	float blue_y_begin   = 999.0;
	float blue_y_end     = 999.0;
    
    // NOT (outside the map OR in box OR in pillar OR in middle wall OR in white box OR in blue box)
    if (!(x < map_x_begin || x > map_x_end || y < map_y_begin || y > map_y_end
            || (x > box_x_begin    && x < box_x_end    && y > box_y_begin    && y < box_y_end)
            || (x > pillar_x_begin && x < pillar_x_end && y > pillar_y_begin && y < pillar_y_end)
            || (x > wall_x_begin   && x < wall_x_end   && y > wall_y_begin   && y < wall_y_end)
            || (x > white_x_begin  && x < white_x_end  && y > white_y_begin  && y < white_y_end)
            || (x > blue_x_begin   && x < blue_x_end   && y > blue_y_begin   && y < blue_y_end) )) 
    {
        //Check if the new coordinate is inside the radius of any of the filtering table coordinates
        for (float2DVector::size_type i = 0; i < filteringTable.size(); i++) {
            dist = sqrt(pow(x - filteringTable[i][0],2) + pow(y - filteringTable[i][1],2));
            if (dist < SAME_TOY_MAX_R) {
                //Inside coordinate radius, update the filtering table coordinate
                filteringTable[i][0] = (filteringTable[i][0] + x) / 2;
                filteringTable[i][1] = (filteringTable[i][1] + y) / 2;
                filteringTable[i][3] = filteringTable[i][3] + 1;
                if (filteringTable[i][3] >= INPUT_TRUST_TRESHOLD) {
                    //coordinate has reached it input trust treshold, update toys
                    update_toys(filteringTable[i][0], filteringTable[i][1],filteringTable[i][2]);
                }
                found = true;
                break;
            }
        }
        if (!found) {
            //no matching toy, add a new toy to filtering table
            vector<float> pos;
            pos.push_back(x);
            pos.push_back(y);
            pos.push_back(input_coord->z);
            pos.push_back(1);
            filteringTable.push_back(pos);

            //If too much coordinates in filtering table, clear first ones
            while (filteringTable.size() > 50) {
                filteringTable.erase(filteringTable.begin());
            }
        }
    }
    
}