<commit_before>import driver
# from driver import heading_to_quaternion, quaternion_to_heading
# from driver import dot_product, cross_product, scale, unit
# import rospy
from geometry_msgs.msg import Twist

class ExampleDriver(driver.Driver):
    smoothness = 10
    k = 1.0/smoothness
    a = 3*k
    b = 3*pow(k, 2)
    c = pow(k, 3)

    max_v = 1.0
    max_accel = .05
    max_omega = 0.5
    max_alpha = .05

    # pylint: disable=no-self-use
    # incorrectly identifies helpers as no-self-use

    def __init__(self):
        super(ExampleDriver, self).__init__()
        self.last_odom = None

    def process_odom(self, odom):
        """
        Converts new best estimate of position to command
        Based on theory from:
        <NAME> and <NAME>
        "A New Line Tracking Method for Non-Holonomic Vehicles,"
        in International Conference on Robotics and Automation,
        Albuquerque, NM, 1997, pp.2908-2913
        """
        if self.last_odom is None:
            self.last_odom = odom

        next_goal = self.lead_goal()
        if self.dist(next_goal, odom) < .04:
            # if you are .04 m or less from the goal, move forward
            # NOTE: this might change.
            # for example, you may want to look at the next_goal two points
            #  ahead and see if you want to skip the current one, or if you are
            #  ahead of the current one in the direction that you want to go
            next_goal = self.lead_next()

        # errors along axis "x", off axis "y", heading "theta"
        along, off, heading = self.calc_errors(odom, next_goal)

        # d(kurvature)/ds = -a(kurvature) - b(heading_error) - c(off_error)
        # kurvature is defined as: 1/instant radius
        # v = rw
        # w = v/r
        # instant radius = v/w

        # choose a, b, c based on:
        # a = 3k
        # b = 3(k^2)
        # c = k^3
        # choose k based on smoothness = 1/k

        # implement the math
        last_radius = self.calc_old_radius(odom.twist.twist.linear.x,
            odom.linear.twist.twist.angular.z)

        kurvature = self.kurvature_from_radius(last_radius)

        dtime = odom.header.time.secs - self.last_odom.header.time.secs
        delta_kurv_discrete = (-1.0*self.a*kurvature -
            self.b*heading - self.c*off)*dtime

        new_kurvature = kurvature+delta_kurv_discrete
        new_radius = self.radius_from_kurvature(new_kurvature)

        # match speed to goal, with adjustment for position error
        linear_vel = next_goal.twist.twist.linear.x
        linear_vel += -0.5*along

        linear_vel = self.check_linear_limits(odom, linear_vel)

        # calculate angular velocity
        angular_vel = linear_vel / new_radius

        angular_vel = self.check_angular_limits(odom, angular_vel)
        # reset linear_vel based on new (lower, limited) angular vel
        linear_vel = angular_vel * new_radius

        # linear and angular velocity are now within dx/dt, d2x/dt2 limits
        twist_out = Twist()
        twist_out.linear.x = linear_vel
        twist_out.angular.z = angular_vel
        self.cmd_vel.publish(twist_out)

    def calc_old_radius(self, linear_vel, angular_vel):
        if angular_vel < .001:
            last_radius = float("inf")
        else:
            last_radius = abs(linear_vel) / abs(angular_vel)
        return last_radius

    def kurvature_from_radius(self, radius):
        if radius < .001:
            kurvature = 1000 # upper bound on kurvature
        else:
            kurvature = 1.0 / radius
        return kurvature

    def radius_from_kurvature(self, k):
        if k < .001:
            new_radius = float("inf")
        else:
            new_radius = 1.0/k
        return new_radius

    def check_linear_limits(self, odom, linear_vel):
        # limit maximum acceleration
        if odom.twist.twist.linear.x + self.max_accel < linear_vel:
            linear_vel = odom.twist.twist.linear.x + self.max_accel
        elif linear_vel < odom.twist.twist.linear.x - self.max_accel:
            linear_vel = odom.twist.twist.linear.x - self.max_accel

        # limit maximum speed
        if linear_vel > self.max_v:
            linear_vel = self.max_v
        elif linear_vel < -1.0*self.max_v:
            linear_vel = -1.0*self.max_v

        return linear_vel

    def check_angular_limits(self, odom, angular_vel):
        # limit maximum angular acceleration
        if odom.twist.twist.angular.z + self.max_alpha < angular_vel:
            angular_vel = odom.twist.twist.angular.z + self.max_alpha
        elif angular_vel < odom.twist.twist.angular.z - self.max_alpha:
            angular_vel = odom.twist.twist.angular.z - self.max_alpha

        # limit maximum angular speed
        if angular_vel > self.max_omega:
            angular_vel = self.max_omega
        elif angular_vel < -1.0*self.max_omega:
            angular_vel = -1.0*self.max_omega

if __name__ == '__main__':
    # pylint: disable=invalid-name
    # ignoring the trivial naming of the ExampleDriver class for startup and run
    d = ExampleDriver()
    d.run_node()<commit_msg>Clean up Example Driver with comments<commit_after>"""
Example Driver
Example implementation of the driver

process_odom is the key method here
"""

import driver
# from driver import heading_to_quaternion, quaternion_to_heading
# from driver import dot_product, cross_product, scale, unit
# import rospy
from geometry_msgs.msg import Twist

class ExampleDriver(driver.Driver):
    """
    See module docstring
    """
    smoothness = 10
    k = 1.0/smoothness
    a = 3*k
    b = 3*pow(k, 2)
    c = pow(k, 3)

    max_v = 1.0
    max_accel = .05
    max_omega = 0.5
    max_alpha = .05

    # pylint: disable=no-self-use
    # incorrectly identifies helpers as no-self-use

    def __init__(self):
        super(ExampleDriver, self).__init__()
        self.last_odom = None

    def process_odom(self, odom):
        """
        Converts new best estimate of position to command
        Based on theory from:
        <NAME> and <NAME>
        "A New Line Tracking Method for Non-Holonomic Vehicles,"
        in International Conference on Robotics and Automation,
        Albuquerque, NM, 1997, pp.2908-2913
        """
        if self.last_odom is None:
            self.last_odom = odom

        next_goal = self.lead_goal()
        if self.dist(next_goal, odom) < .04:
            # if you are .04 m or less from the goal, move forward
            # NOTE: this might change.
            # for example, you may want to look at the next_goal two points
            #  ahead and see if you want to skip the current one, or if you are
            #  ahead of the current one in the direction that you want to go
            next_goal = self.lead_next()

        # errors along axis "x", off axis "y", heading "theta"
        along, off, heading = self.calc_errors(odom, next_goal)

        # d(kurvature)/ds = -a(kurvature) - b(heading_error) - c(off_error)
        # kurvature is defined as: 1/instant radius
        # v = rw
        # w = v/r
        # instant radius = v/w

        # choose a, b, c based on:
        # a = 3k
        # b = 3(k^2)
        # c = k^3
        # choose k based on smoothness = 1/k

        # implement the math
        last_radius = self.calc_old_radius(odom.twist.twist.linear.x,
            odom.linear.twist.twist.angular.z)

        kurvature = self.kurvature_from_radius(last_radius)

        dtime = odom.header.time.secs - self.last_odom.header.time.secs
        delta_kurv_discrete = (-1.0*self.a*kurvature -
            self.b*heading - self.c*off)*dtime

        new_kurvature = kurvature+delta_kurv_discrete
        new_radius = self.radius_from_kurvature(new_kurvature)

        # match speed to goal, with adjustment for position error
        linear_vel = next_goal.twist.twist.linear.x
        linear_vel += -0.5*along

        linear_vel = self.check_linear_limits(odom, linear_vel)

        # calculate angular velocity
        angular_vel = linear_vel / new_radius

        angular_vel = self.check_angular_limits(odom, angular_vel)
        # reset linear_vel based on new (lower, limited) angular vel
        linear_vel = angular_vel * new_radius

        # linear and angular velocity are now within dx/dt, d2x/dt2 limits
        twist_out = Twist()
        twist_out.linear.x = linear_vel
        twist_out.angular.z = angular_vel
        self.cmd_vel.publish(twist_out)

    def calc_old_radius(self, linear_vel, angular_vel):
        """
        Calculate an instant radius from a linear velocity and angular velocity
        """
        if angular_vel < .001:
            last_radius = float("inf")
        else:
            last_radius = abs(linear_vel) / abs(angular_vel)
        return last_radius

    def kurvature_from_radius(self, radius):
        """
        Calculate the instant kurvature from an instant radius
        """
        if radius < .001:
            kurvature = 1000 # upper bound on kurvature
        else:
            kurvature = 1.0 / radius
        return kurvature

    def radius_from_kurvature(self, k):
        """
        Calculate the instant radius from the kurvature
        """
        if k < .001:
            new_radius = float("inf")
        else:
            new_radius = 1.0/k
        return new_radius

    def check_linear_limits(self, odom, linear_vel):
        """
        Make sure the given linear velocity fits within accel/speed limits
        """
        # limit maximum acceleration
        if odom.twist.twist.linear.x + self.max_accel < linear_vel:
            linear_vel = odom.twist.twist.linear.x + self.max_accel
        elif linear_vel < odom.twist.twist.linear.x - self.max_accel:
            linear_vel = odom.twist.twist.linear.x - self.max_accel

        # limit maximum speed
        if linear_vel > self.max_v:
            linear_vel = self.max_v
        elif linear_vel < -1.0*self.max_v:
            linear_vel = -1.0*self.max_v

        return linear_vel

    def check_angular_limits(self, odom, angular_vel):
        """
        Make sure the given angular velocity fits within accel/speed limits
        """
        # limit maximum angular acceleration
        if odom.twist.twist.angular.z + self.max_alpha < angular_vel:
            angular_vel = odom.twist.twist.angular.z + self.max_alpha
        elif angular_vel < odom.twist.twist.angular.z - self.max_alpha:
            angular_vel = odom.twist.twist.angular.z - self.max_alpha

        # limit maximum angular speed
        if angular_vel > self.max_omega:
            angular_vel = self.max_omega
        elif angular_vel < -1.0*self.max_omega:
            angular_vel = -1.0*self.max_omega

if __name__ == '__main__':
    # pylint: disable=invalid-name
    # ignoring the trivial naming of the ExampleDriver class for startup and run
    d = ExampleDriver()
    d.run_node()
{'fraction_non_alphanumeric': 0.08448893378638994, 'fraction_numerical': 0.01267334006795849, 'mean_word_length': 2.1437644341801385, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2941585', 'n_tokens_mistral': 3564, 'n_tokens_neox': 3349, 'n_words': 911}