float evalSafety(geometry_msgs::Point &comand, geometry_msgs::Pose2D &target, nav_msgs::Odometry &robotPos, sensor_msgs::LaserScan &currScan)
{
float safety = -1;
double ang_min, dist_min;
double alfa = atan2(comand.y, comand.x);
bool isOk = getMinDist(currScan, &dist_min, &ang_min);
//ROS_INFO("Min dist is (%f), min angle is (%f)",dist_min,ang_min);
if (isOk) {
safety = getSafety(ang_min, dist_min, alfa);
}
return safety;
}
float evalSafety(geometry_msgs::Point &comand, geometry_msgs::Pose2D &target,
nav_msgs::Odometry &robotPos, nav_msgs::GridCells obstacles)
{
float safety = -1;
double ang_min, dist_min;
double alfa = atan2(comand.y, comand.x);
bool isOk = getMinDist(obstacles,robotPos, &dist_min, &ang_min);
//ROS_INFO("Min dist is (%f), min angle is (%f)",dist_min,ang_min);
if (isOk) {
safety = getSafety(ang_min, dist_min, alfa);
}
return safety;
}
int Party::getNextColour(int x, int y)
{
int limit = 0;
int color_limit = check.size();
for(int k = 0; k < (int)colours.size(); ++k){
int dist = getMinDist(colours[k], x, y);
if(dist > limit){
limit = dist;
color_limit = (int)check.size();
}
if(dist == limit){
if(colours[k] < color_limit){
color_limit = colours[k];
}
}
}
return color_limit;
}
