// 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; }