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