def get_velocity(self, pose, vel):
        self.state.update(pose.x,
                          pose.y,
                          pose.theta,
                          vel,
                          is_backward=self.direction)

        desired = Pose()
        if self.last_idx > self.target_idx:
            self.is_at_goal = False

            # Calc control input
            ai = self.pid_control(self.max_linear_speed, self.state.v)
            di, self.target_idx = self.pure_pursuit_steer_control(
                self.state, self.target_course, self.target_idx)

            desired.xVel = self.state.v + ai

            desired.thetaVel = abs(self.state.v) / self.wheel_base * np.tan(di)
            self.test_theta_vel = abs(
                self.state.v) / self.wheel_base * np.tan(di)

        else:
            if (self.ignore_angular_tolerance):
                ai = self.pid_control(self.max_linear_speed, self.state.v)
                desired.xVel = self.state.v + ai

            self.is_at_goal = True

        return desired
    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
    def getVelocity(self, cur, goal, dT):
        desired = Pose()

        a = -cur.theta + atan2(goal.y - cur.y, goal.x - cur.x)

        # In Automomous Mobile Robots, they assume theta_G=0. So for
        # the error in heading, we have to adjust theta based on the
        # (possibly non-zero) goal theta.
        theta = cur.theta - goal.theta
        b = -theta - a

        d = self.getGoalDistance(cur, goal)
        if self.forwardMovementOnly:
            direction = 1
            a = self.normalizePi(a)
            b = self.normalizePi(b)
            #if abs(a) > pi/2:
            #    b = 0
        else:
            direction = self.sign(cos(a))
            a = self.normalizeHalfPi(a)
            b = self.normalizeHalfPi(b)

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

        #print('current x:', cur.x, 'y:', cur.y, 'theta:', cur.theta,
        #    '  goal x:', goal.x, 'y:', goal.y, 'theta:', goal.theta)
        #print('  theta:', theta, 'a:', a, 'b:', b,
        #    'v:', desired.xVel, 'w:', desired.thetaVel)
        #print(str(cur.x) + ',' + str(cur.y) + ',' + str(cur.theta))

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

        return desired
    def get_velocity(self, cur, goal, dT):
        desired = Pose()

        goal_heading = atan2(goal.y - cur.y, goal.x - cur.x)
        a = -cur.theta + goal_heading

        # In Automomous Mobile Robots, they assume theta_G=0. So for
        # the error in heading, we have to adjust theta based on the
        # (possibly non-zero) goal theta.
        theta = self.normalize_pi(cur.theta - goal.theta)
        b = -theta - a

        # rospy.loginfo('cur=%f goal=%f a=%f b=%f', cur.theta, goal_heading,
        #               a, b)

        d = self.get_goal_distance(cur, goal)
        if self.forward_movement_only:
            direction = 1
            a = self.normalize_pi(a)
            b = self.normalize_pi(b)
        else:
            direction = self.sign(cos(a))
            a = self.normalize_half_pi(a)
            b = self.normalize_half_pi(b)

        # rospy.loginfo('After normalization, a=%f b=%f', a, b)

        if self.within_linear_tolerance:
            desired.xVel = 0
            desired.thetaVel = self.kB * theta
        else:
            desired.xVel = self.kP * d * direction
            desired.thetaVel = self.kA * a + self.kB * b

        # Adjust velocities if X velocity is too high.
        if abs(desired.xVel) > self.max_linear_speed:
            ratio = self.max_linear_speed / abs(desired.xVel)
            desired.xVel *= ratio
            #desired.thetaVel *= ratio

        # Adjust velocities if turning velocity too high.
        if abs(desired.thetaVel) > self.max_angular_speed:
            ratio = self.max_angular_speed / abs(desired.thetaVel)
            #desired.xVel *= ratio
            desired.thetaVel *= ratio

        # TBD: Adjust velocities if linear or angular acceleration
        # too high.

        # Adjust velocities if too low, so robot does not stall.
        if abs(desired.xVel) > 0 and abs(desired.xVel) < self.min_linear_speed:
            ratio = self.min_linear_speed / abs(desired.xVel)
            desired.xVel *= ratio
            #desired.thetaVel *= ratio
        if desired.xVel == 0 and abs(
                desired.thetaVel) < self.min_angular_speed:
            ratio = self.min_angular_speed / abs(desired.thetaVel)
            #desired.xVel *= ratio
            desired.thetaVel *= ratio
        #print("Theta vel:", str(desired.thetaVel))
        #print("Min theta:", str(self.min_angular_speed))
        #print("Linear vel:", str(desired.xVel))
        #print("Ratio:", str(ratio))
        return desired