void Costmap2D::updateWorld(double robot_x, double robot_y,
const vector<Observation>& observations, const vector<Observation>& clearing_observations){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
//raytrace freespace
raytraceFreespace(clearing_observations);
//if we raytrace X meters out... we must re-inflate obstacles within the containing square of that circle
double inflation_window_size = 2 * (max_raytrace_range_ + inflation_radius_);
//clear all non-lethal obstacles in preparation for re-inflation
clearNonLethal(robot_x, robot_y, inflation_window_size, inflation_window_size);
//reset the inflation window
resetInflationWindow(robot_x, robot_y, inflation_window_size + 2 * inflation_radius_, inflation_window_size + 2 * inflation_radius_, inflation_queue_, false);
//now we also want to add the new obstacles we've received to the cost map
updateObstacles(observations, inflation_queue_);
inflateObstacles(inflation_queue_);
}
void
SSLPathPlanner::globalStateRcvd (const ssl_msgs::GlobalState::ConstPtr &global_msg)
{
if (!global_st_rcvd)
global_st_rcvd = true;
global_state = *global_msg;
if (team_ == ssl::BLUE)
{
for (uint8_t i = 0; i < ssl::config::TEAM_CAPACITY; i++)
team_state_[i] = global_state.blue_team[i];
}
else
{
for (uint8_t i = 0; i < ssl::config::TEAM_CAPACITY; i++)
team_state_[i] = global_state.yellow_team[i];
}
updateObstacles ();
}
void VO2Controller::update(float dt)
{
updatePath(dt);
updateObstacles(0);
float maxVelocity = mover->getMaxVelocity(0);
// Определяем, как далеко осталось до ближайшего вейпоинта и
// выбираем соответствующую стратегию движения
//Pose::vec targetDir = this->pathDirection();
//float distance=targetDir.length();
//const float targetCloseFactor = 0.2f;
//vec2f size = mover->getUnit()->getOOBB().size();
//float sphereSize=1.3 * mover->getUnit()->getBoundingSphere().radius;
float turningSpeed = mover->getMaxVelocity(1);
float turningRadius = maxVelocity / turningSpeed;
Pose moverPose = mover->getGlobalPose();
Pose::vec velocity = Pose::vec::zero();
Pose::vec preferedDirection = Pose::vec::zero();
float preferedAngle = 0;
float preferedSpeed = 0;
bool findMax = true;
bool turnOnly = false;
if( pathCurrent != -1 )
{
Pose::vec pathDir = pathDirection();
Pose::vec targ = targetScale * pathDir;
Pose::vec path = pathErrorScale * pathError();
if( fabs( pathDir[0] ) < 2 * turningRadius && fabs( pathDir[1] ) < 2*turningRadius)
{
float x = pathDir & moverPose.axisX();
float y = fabs(pathDir & moverPose.axisY()) - turningRadius;
turnOnly = ( x*x + y*y < turningRadius*turningRadius );
}
preferedDirection = normalise(path + targ);
preferedAngle = atan2(preferedDirection[1], preferedDirection[0]); // а не пойти ли нам вон туда
preferedSpeed = maxVelocity; // и побыстрее
MoverVehicle * mv = (MoverVehicle*)mover;
if(mv->definition->kinematic)
{
Pose::vec bodyVelocityLinear = conv(mover->getBody()->GetLinearVelocity());
float bodyVelocityAngular = mover->getBody()->GetAngularVelocity();
/*
velProj = (vel & pathDir) / |pathDir|
time = |pathDir| / |velProj| = |pathDir| * |pathDir| / (vel & pathDir)
*/
//float distanceLeft = bodyVelocityLinear & pathDir.normalise();
/*
float arriveTime = pathDir.length_squared() / (bodyVelocityLinear & pathDir);
float brakeTime = bodyVelocityLinear.length() / mv->definition->acceleration[0];
if(arriveTime > 0 && arriveTime < brakeTime)
{
preferedSpeed = arriveTime * mv->definition->acceleration[0];
if(preferedSpeed > maxVelocity)
preferedSpeed = maxVelocity;
}*/
}
}
else
{
findMax = false;
preferedSpeed = 0; // хотелось бы стоять на месте и не двигаться
preferedAngle = mover->getGlobalPose().orientation; // и смотреть в ту же сторону
}
float bestRate = 2 * maxVelocity; // лучший рейтинг, для начала берём самый худший
Pose::vec bestVelocity = Pose::vec::zero(); // лучшая скорость
// choose best segment
::rayCast(velocitySpace,preferedAngle,steps,rays);
/*
сначала отсечь всё промежутком [0,maxVelocity],
затем надо перебрать все оставшиеся промежутки, и выбрать
максимальную границу.
*/
std::vector<float> bestRanges;
if(findMax)
getMaxRanges(rays, preferedSpeed, bestRanges);
else
getMinRanges(rays, 0, bestRanges);
/*
затем выбираем наилучшее направление
*/
vec2f preferedVelocity = preferedDirection * preferedSpeed;
for(int i = 0; i < rays.size();i++)
{
float angle = rays[i].first;
float distance = bestRanges[i];
if( distance < 0 )
continue;
Pose::vec vel = distance * Pose::vec(cosf(angle),sinf(angle));
float rate = vecDistance(vel, preferedVelocity);
if(distance > 0 && rate < bestRate)
{
bestRate = rate;
bestVelocity = vel;
}
}
velocity = bestVelocity;
float approachTime=2.0;
float time = timeToImpact();
if(time<approachTime && time>0)
velocity*=(time/approachTime);
const float t = 0.5; // варьируем инертность объекта.
mover->directionControl(velocity, turnOnly ? 0.f : velocity.length());
//object->newVelocity=object->velocity*(1.f-t)+velocity*t;
}
