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()); } }