/*
Remove toy with input index from knownToys
*/
void removeToy(float index)
{
int int_index = int(index);
knownToys.erase(knownToys.begin()+int_index);
}
/*
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());
}
}
}
}