void OmniDrivePositionController::targetReached(bool& translation, bool& rotation) {
KDL::Trajectory& trajectoryComposite = targetTrajectory.getTrajectory();
double duration = trajectoryComposite.Duration();
KDL::Frame frame = trajectoryComposite.Pos(duration);
double roll, pitch, yaw;
frame.M.GetRPY(roll, pitch, yaw);
Pose2D desiredPose(frame.p.x(), frame.p.y(), yaw);
Pose2D actualPose = actualOdometry.getPose2D();
double angDist = getShortestAngle(desiredPose.getTheta(), actualPose.getTheta());
double linDist = getDistance(desiredPose, actualPose);
translation = false;
rotation = false;
if (linDist <= tolerance.getPose2D().getX() && linDist <= tolerance.getPose2D().getY()) {
translation = true;
}
if (fabs(angDist) <= tolerance.getPose2D().getTheta()) {
rotation = true;
}
}
void MovableObject::update(const Pose2D& pose)
{
Pose2D previousPose = _pose;
// Call update of visible object
VisibleObject::update(pose);
// Calculate object speed
_speed = _pose.getPosition() - previousPose.getPosition();
}
const Pose2D Pose2D::interpolate(const Pose2D& startPose,
const Pose2D& endPose,
const double& progress)
{
if (progress <= 0) {
return startPose;
} else if (progress >= 1) {
return endPose;
}
Vector2d diff = endPose.getPosition() - startPose.getPosition();
Angle angle = Angle::interpolate(startPose.getAngle(), endPose.getAngle(), progress);
return Pose2D(startPose.getPosition() + (diff * progress), angle);
}
void refBoxTransport::set_pose(double x, double y, double theta)
{
m_dataMutex.lock();
unsigned long sec = 0;
unsigned long nsec = 0;
get_actualTime(sec, nsec);
Pose2D *pose = m_beaconSignal.mutable_pose();
pose->set_x(x);
pose->set_y(y);
pose->set_ori(theta);
Time *pose_time = pose->mutable_timestamp();
pose_time->set_sec(static_cast<google::protobuf::int64>(sec));
pose_time->set_nsec(static_cast<google::protobuf::int64>(nsec));
m_dataMutex.unlock();
}
// ========== STATIC HELPER METHODS ===========
const Pose2D Pose2D::average(const Pose2D& p1,
const Pose2D& p2,
const double& weight1,
const double& weight2)
{
double divider = weight1 + weight2;
double x = (p1.x() * weight1 + p2.x() * weight2) / divider;
double y = (p1.y() * weight1 + p2.y() * weight2) / divider;
return Pose2D(x, y, Angle::average(p1.getAngle(), p2.getAngle(), weight1, weight2));
}
Vector2d DifferentialDriveKinematics::PartialInverse(const Pose2D<double> &a, const Pose2D<double> &b) {
Vector2d diff;
diff << b.x() - a.x(), b.y() - a.y();
// This rotation doesn't affect the magnitude of ds,
// but it so the sign can work out properly
Vector2d dr = a.rotationMatrix().inverse()*diff;
double ds = sqrt(dr(0)*dr(0)+dr(1)*dr(1));
if ( dr(0) < 0.0 ) {
ds = -ds;
}
double dtheta = b.heading() - a.heading(); // difference in headings
ecl::wrap_angle(dtheta);
// diff << ds, dtheta;
return (diff << ds, dtheta).finished();
}
Pose2D operator+(const Pose2D& op1, const Pose2D& op2) {
Pose2D pose(op1.getX() + op2.getX(), op1.getY() + op2.getY(), op1.getTheta() + op2.getTheta());
return pose;
}
Pose2D::Pose2D(const Pose2D& orig) : x(orig.getX()), y(orig.getY()), theta(orig.getTheta()) {
}
float OmniDrivePositionController::getDistance(const Pose2D& actualPose, const Pose2D& goalPose) {
return sqrt((actualPose.getX() - goalPose.getX()) * (actualPose.getX() - goalPose.getX()) +
(actualPose.getY() - goalPose.getY()) * (actualPose.getY() - goalPose.getY()));
}
const Angle Pose2D::getAngleTo(const Pose2D& other) const
{
return getAngleTo(other.getPosition());
}
const double Pose2D::getDistanceTo(const Pose2D& other) const
{
double deltax = _position(0) - other._position(0);
double deltay = _position(1) - other._position(1);
return std::sqrt(deltax * deltax + deltay * deltay);
}
const Pose2D Pose2D::operator+(const Pose2D& other) const
{
return Pose2D(getPosition() + other.getPosition(), getAngle() + other.getAngle());
}
void StreetRenderer::drawSegment(QPainter& painter,
A2O::Segment::Ptr segment,
const PanelTransformation& panelTransform)
{
Pose2D beginPose = segment->getBeginLink()->getPose();
Pose2D endPose = Geometry::calculateArcPose(segment->getLength(), segment->getBendingAngle());
Angle bendingAngle = segment->getBendingAngle();
bool straightSegment = segment->getBendingAngle().rad() == 0;
double radius = 0;
// Set the current painting system to the begin of the segment
panelTransform.setTransform(beginPose, painter);
QPointF begin(0, 0);
QPointF end(endPose.getPosition()(0), endPose.getPosition()(1));
// Draw segment background
if (straightSegment) {
// Draw street background
if (segment->consumesDriveInstruction()) {
painter.setPen(_instructionStreetPen);
} else {
painter.setPen(_streetPen);
}
painter.drawLine(begin, end);
} else {
radius = double(segment->getLength() / bendingAngle.rad());
// Draw street background
if (segment->consumesDriveInstruction()) {
painter.setPen(_instructionStreetPen);
} else {
painter.setPen(_streetPen);
}
RenderUtil::drawArc(painter, begin, radius, bendingAngle.rad());
painter.setPen(_thinStreetPen);
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad());
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad());
}
// Draw Signs belonging to segment
std::vector<RoadSign::Ptr> signs = segment->getRoadSigns();
for(unsigned int i=0; i < signs.size(); i++) {
RoadSign::Ptr sign = signs[i];
std::stringstream fileName;
fileName << ":/res/" << sign->getName() << ".png";
std::string file = fileName.str();
QImage image(file.c_str());
Angle signAngle = sign->getPose().getAngle();
signAngle -= Angle::deg(90);
image = image.transformed(QTransform().rotate(signAngle.deg()));
image = image.mirrored(false, true);
QRectF rect(0.0, -0.25, 0.5, 0.5);
painter.drawImage(rect, image);
}
// Draw begin link and sub segments
drawSubSegments(painter, segment, panelTransform);
panelTransform.setTransform(beginPose, painter);
// Draw segment lines
if (straightSegment) {
// Draw segment lines
painter.setPen(_streetLinePen);
if (segment->isXCrossing()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(1, 0.465), QPointF(0.965, 0.5));
painter.drawLine(QPointF(1, -0.465), QPointF(0.965, -0.5));
} else if(segment->isTCrossingLS()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(1, 0.465), QPointF(0.965, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(1, -0.465));
} else if(segment->isTCrossingLR()) {
painter.drawLine(QPointF(0, 0.465), QPointF(0.035, 0.5));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(0.965, 0.5), QPointF(0.965, -0.5));
} else if(segment->isTCrossingSR()) {
painter.drawLine(QPointF(0, 0.465), QPointF(1, 0.465));
painter.drawLine(QPointF(0, -0.465), QPointF(0.035, -0.5));
painter.drawLine(QPointF(1, -0.465), QPointF(0.965, -0.5));
} else {
painter.drawLine(QPointF(0, 0.465), QPointF(segment->getLength(), 0.465));
painter.drawLine(QPointF(0, -0.465), QPointF(segment->getLength(), -0.465));
// Draw middle line
painter.setPen(_middleLinePen);
painter.drawLine(begin, end);
}
} else {
// Draw left and right line
painter.setPen(_streetLinePen);
if (radius > 0) {
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad());
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad(), _helperLinePen);
} else {
RenderUtil::drawArc(painter, QPointF(0, -0.465), radius + 0.465, bendingAngle.rad(), _helperLinePen);
RenderUtil::drawArc(painter, QPointF(0, 0.465), radius - 0.465, bendingAngle.rad());
}
// Draw middle line
painter.setPen(_middleLinePen);
RenderUtil::drawArc(painter, begin, radius, bendingAngle.rad());
}
}
