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