/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// normalise desired velocity vector
Vector2f vel_dir = desired_vel.normalized();
// get angle of desired velocity
float heading_rad = atan2f(vel_dir.y, vel_dir.x);
// rotate desired velocity angle into body-frame angle
float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);
// get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
float distance_m;
if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
}
}
void RRTWidget::drawTerminalState(QPainter &painter, const Vector2f &pos, const Vector2f &vel, const QColor &color) {
// draw point
painter.setPen(QPen(color, 6));
QPointF rootLoc(pos.x(), pos.y());
painter.drawEllipse(rootLoc, 2, 2);
Vector2f tipOffset = vel * VelocityDrawingMultiplier;
Vector2f tipLocVec = pos + tipOffset;
QPointF tipLoc(tipLocVec.x(), tipLocVec.y());
// draw arrow shaft
painter.setPen(QPen(color, 3));
painter.drawLine(rootLoc, tipLoc);
// draw arrow head
Vector2f headBase = tipLocVec - tipOffset.normalized()*4;
Vector2f perp = Vector2f(-tipOffset.y(), tipOffset.x()).normalized();
Vector2f tipLeftVec = headBase + perp*4;
Vector2f tipRightVec = headBase - perp*4;
QPointF trianglePts[] = {
tipLoc,
QPointF(tipLeftVec.x(), tipLeftVec.y()),
QPointF(tipRightVec.x(), tipRightVec.y())
};
painter.drawPolygon(trianglePts, 3);
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
bool AR_WPNav::get_stopping_location(Location& stopping_loc)
{
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
return false;
}
// get current velocity vector and speed
const Vector2f velocity = AP::ahrs().groundspeed_vector();
const float speed = velocity.length();
// avoid divide by zero
if (!is_positive(speed)) {
stopping_loc = current_loc;
return true;
}
// get stopping distance in meters
const float stopping_dist = _atc.get_stopping_distance(speed);
// calculate stopping position from current location in meters
const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
stopping_loc = current_loc;
stopping_loc.offset(stopping_offset.x, stopping_offset.y);
return true;
}
bool MidCirclePerceptor::searchWithSXAndT(MidCircle& midCircle) const
{
std::vector<const FieldLineIntersections::Intersection*> useSmallXIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::X && intersection.additionalType == FieldLineIntersections::Intersection::none)
useSmallXIntersections.push_back(&intersection);
if(useSmallXIntersections.empty())
return false;
static const float distance = theFieldDimensions.yPosLeftSideline - theFieldDimensions.centerCircleRadius ;
std::vector<const FieldLineIntersections::Intersection*> useTIntersections;
for(const FieldLineIntersections::Intersection& intersection : theFieldLineIntersections.intersections)
if(intersection.type == FieldLineIntersections::Intersection::T)
useTIntersections.push_back(&intersection);
for(const FieldLineIntersections::Intersection* intersectionSX : useSmallXIntersections)
for(const FieldLineIntersections::Intersection* intersectionT : useTIntersections)
if(intersectionT->indexDir1 == intersectionSX->indexDir1 || intersectionT->indexDir1 == intersectionSX->indexDir2)
if(std::abs((intersectionT->pos - intersectionSX->pos).norm() - distance) < allowedTsXVariance)
{
const Vector2f dirTToX = intersectionSX->pos - intersectionT->pos;
midCircle.translation = intersectionSX->pos + dirTToX.normalized(theFieldDimensions.centerCircleRadius);
midCircle.rotation = Angle(dirTToX.angle() + pi_2).normalize();// OR intersectionT->dir2.angle();
midCircle.markedPoints.emplace_back(midCircle.translation, MarkedPoint::midCircle);
theIntersectionRelations.propagateMarkedIntersection(MarkedIntersection(intersectionT->ownIndex, MarkedIntersection::BT),
theFieldLineIntersections, theFieldLines, midCircle);
return true;
}
return false;
}
//----------------------------------------------------------------------------------------
void Entity::update(float deltaSeconds)
{
Vector2f velocity = m_target - m_position;
if (!(velocity == Vector2f(0,0)))
{
velocity = velocity.normalized();
m_position = m_position + (velocity * deltaSeconds * 50.f);
}
}
Obstacle::Obstacle(const Matrix2f& covariance, const Vector2f& center, unsigned int lastSeen, Type type) :
covariance(covariance), center(center), velocity(Vector2f::Zero()), lastSeen(lastSeen), type(type)
{
float radius = 55.f;
if(type != Obstacle::goalpost)
radius = getRobotDepth();
else if(Blackboard::getInstance().exists("FieldDimensions"))
radius = static_cast<const FieldDimensions&>(Blackboard::getInstance()["FieldDimensions"]).goalPostRadius;
left = right = center.normalized(radius);
left.rotateLeft();
right.rotateRight();
left += center;
right += center;
}
MTV getMTV_circ_circ(Game_Object &circle0, Game_Object &circle1){
MTV result;
float overlap = 5000.0f;
Vector2f smallest;
const Vector2f dist_vec = (circle0.get_position() + 0.5f * circle0.get_size()) -
(circle1.get_position() + 0.5f * circle1.get_size());
const float dist2 = dist_vec * dist_vec;
const float radius_sum = circle0.get_radius() + circle1.get_radius();
if (dist2 < radius_sum * radius_sum) {
overlap = radius_sum - sqrt(dist2);
smallest = dist_vec.normalized();
result = MTV(true, smallest, overlap);
}
return result;
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
void Mode::calc_stopping_location(Location& stopping_loc)
{
// default stopping location
stopping_loc = rover.current_loc;
// get current velocity vector and speed
const Vector2f velocity = ahrs.groundspeed_vector();
const float speed = velocity.length();
// avoid divide by zero
if (!is_positive(speed)) {
stopping_loc = rover.current_loc;
return;
}
// get stopping distance in meters
const float stopping_dist = attitude_control.get_stopping_distance(speed);
// calculate stopping position from current location in meters
const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
location_offset(stopping_loc, stopping_offset.x, stopping_offset.y);
}
