// Checks if a robot is going to collide it is positionned at robotPosition.
// robotPosition : Predicted position of the robot.
// robotIndex : Index of the current robot in the robot array.
// Returns 1 if there will be a collision, 0 otherwise.
int checkCollision(double robotPosition[3], int robotIndex)
{
int i=0;
int collision = 0;
Object** bender = getBender(robotPosition); // Bounding shapes of the robot. One as a box, the other as a cylinder.
// Each will be used for collisions with an object of the same type.
Object** otherBender; // Represents the bounding shapes of other robots.
Object* building; // Represents the bounding shape of the buildings.
// Check if it collides with the other robots.
for(i = 0; i<NBROBOTS && collision == 0; i++)
{
if (i != robotIndex)
{
otherBender = getBender(robot[i].position);
if (inCollision(bender[TYPE_CYLINDER], otherBender[TYPE_CYLINDER]))
collision = 1;
free(otherBender[0]);
free(otherBender[1]);
free(otherBender);
}
}
// Check if it collides with buildings.
for(i = 0; i<NBBUILDINGS && collision == 0; i++)
{
building = getBuilding(buildingPosition[i], buildingType[i]);
if (inCollision(bender[building->type], building))
collision = 1;
free(building);
}
free(bender[0]);
free(bender[1]);
free(bender);
return collision;
}
bool AssistedSteering::checkCommand(geometry_msgs::Twist* twist)
{
//boost::recursive_mutex::scoped_lock l(configuration_mutex_);
saturateVelocities(twist);
float lv = twist->linear.x;
float av = twist->angular.z;
//Movement from robot frame (base_link)
//float lin_dist = lv * time_step_;
//float th = av * time_step_;
//float x = 0.0 + lin_dist*cos(th);
//float y = 0.0 + lin_dist*sin(th);
//float dist = sqrt(x*x + y*y);
//float steps = dist/granularity_;
float vel_mag = sqrt(lv*lv);
float steps = (vel_mag*time_step_)/granularity_;
float dt = time_step_ / steps;
float x=0.0, y=0.0, th=0.0;
//Take the laser scan
laser_mutex_.lock();
sensor_msgs::LaserScan laser = laser_scan_;
laser_mutex_.unlock();
int ini = floor(steps/2.0 + 0.5);
for(unsigned int i=ini; i<steps; i++)
{
float lin_dist = lv * dt;
th = th + (av * dt);
//normalization just in case
th = normalizeAngle(th, -M_PI, M_PI);
x = x + lin_dist*cos(th); //cos(th+av*dt/2.0)
y = y + lin_dist*sin(th);
if(inCollision(x, y, &laser))
return false;
}
// Now we have to check that the robot is
// able to stop from the final velocity, and
// and check the possible collisions in the movement
// until the robot stops.
return true;
}
