goal_controller.py 文件源码

def getVelocity(self, cur, goal, dT):
        desired = Pose()
        d = self.getGoalDistance(cur, goal)
        a = atan2(goal.y - cur.y, goal.x - cur.x) - cur.theta
        b = cur.theta + a - goal.theta

        if abs(d) < self.linearTolerance:
            desired.xVel = 0
            desired.thetaVel = -self.kB * b
        else:
            desired.xVel = self.kP * d
            desired.thetaVel = self.kA*a + self.kB*b

        # Adjust velocities if linear or angular rates or accel too high.
        # TBD

        return desired