<commit_before>#!/usr/bin/env python

import leader
import rospy
import math
from nav_msgs.msg import Odometry

from utils import heading_to_quaternion, quaternion_to_heading
from utils import calc_errors, dist, scale, minimize_angle
from utils import unit as to_unit_tuple

from tf import transformations as tft
import tf

def unit(function):
    def modded_function(*args, **varargs):
        val = function(*args, **varargs)
        return to_unit_tuple(val)
    return modded_function

# pylint: disable=no-self-use
# the class functions are used. I'm not sure why it's missing this.

class ForceLeader(leader.Leader):
    # methods to override:
    # generate_initial_path, generate_next_path

    def generate_initial_path(self):
        """
        Path creation for node
        """
        end = self.path_goal().goal
        start = self.path_start().goal
        self.common_path_gen(start, end)

    def generate_next_path(self):
        end = self.path_next().goal
        start = self.path_start().goal
        self.common_path_gen(start, end)

    def common_path_gen(self, start, end):
        """
        generate a new path, either forwards or backwards (rvs == True)
        """
        self.targets = []
        self.targets.append(start)

        # pylint: disable=invalid-name
        # dt, v, w, are accurately describing what I want in this case

        dt = .1



        next_ = start

        errors = calc_errors(next_, end)
        along = errors[0]

        count = 2

        while along < -0.01 and not rospy.is_shutdown():
            if count > 200:
                rospy.loginfo('path overflow')
                break
            current = StateModel(next_)
            force_vector = self.get_force_vector(start, end, next_)
            force_heading = math.atan2(force_vector[1], force_vector[0])
            rospy.loginfo('fhead: '+str(force_heading)[0:5])
            heading_err = minimize_angle(force_heading - current.theta)
            rospy.loginfo('headerr: '+str(heading_err)[0:5])

            # pylint: disable=invalid-name
            # v, w, are accurately describing what I want in this case
            w = heading_err/dt*0.75
            v = 0.55        
            # TODO(buckbaskin): calculate something smart based on vel
            # profile from start to end
            # possibly do vel profile based on distance, slow down proportional to
            #  heading error relative to force field

            # rospy.loginfo('gnxt: '+str(force_heading)+' ... '+str(current.theta))

            count += 1
            next_ = current.sample_motion_model2(v, w, dt)

            odom_next = self.convert_to_odom(next_)

            self.targets.append(next_)

            

            errors = calc_errors(next_, end)
            along = errors[0]
            # rospy.loginfo('alongher alenghi alphabet: '+str(along))

        self.index = 0



    @unit # forces a unit vector to be returned, scaled by weight
    def get_force_vector(self, start, end, current):
        """
        Aggregate the sum of the force vectors for 3 different actors: departing
        the original location, arriving at the destination and traversing
        between the two points.

        There may be more added, and the end output is
        only relevant for its direction, so the weighting is arbitrary and
        relative, where I've fixed the traverse weight to always be 1.

        This is a superposition of 3? differential equations representing a sum
        of forces.
        """
        wdep = self.weighted_depart(start, end, current)
        warr = self.weighted_arrive(start, end, current)<commit_msg>Removing extraneous code<commit_after>#!/usr/bin/env python

import leader
import rospy
import math
from nav_msgs.msg import Odometry

from utils import heading_to_quaternion, quaternion_to_heading
from utils import calc_errors, dist, scale, minimize_angle
from utils import unit as to_unit_tuple

from tf import transformations as tft
import tf

def unit(function):
    def modded_function(*args, **varargs):
        val = function(*args, **varargs)
        return to_unit_tuple(val)
    return modded_function

# pylint: disable=no-self-use
# the class functions are used. I'm not sure why it's missing this.

class ForceLeader(leader.Leader):
    # methods to override:
    # generate_initial_path, generate_next_path

    def generate_initial_path(self):
        """
        Path creation for node
        """
        end = self.path_goal().goal
        start = self.path_start().goal
        self.common_path_gen(start, end)

    def generate_next_path(self):
        end = self.path_next().goal
        start = self.path_start().goal
        self.common_path_gen(start, end)

    def common_path_gen(self, start, end):
        """
        generate a new path, either forwards or backwards (rvs == True)
        """
        self.targets = []
        self.targets.append(start)

        # pylint: disable=invalid-name
        # dt, v, w, are accurately describing what I want in this case

        dt = .1



        next_ = start

        errors = calc_errors(next_, end)
        along = errors[0]

        count = 2

        while along < -0.01 and not rospy.is_shutdown():
            if count > 200:
                rospy.loginfo('path overflow')
                break
            current = StateModel(next_)
            force_vector = self.get_force_vector(start, end, next_)
            force_heading = math.atan2(force_vector[1], force_vector[0])
            rospy.loginfo('fhead: '+str(force_heading)[0:5])
            heading_err = minimize_angle(force_heading - current.theta)
            rospy.loginfo('headerr: '+str(heading_err)[0:5])

            # pylint: disable=invalid-name
            # v, w, are accurately describing what I want in this case
            w = heading_err/dt*0.75
            v = 0.55        
            # TODO(buckbaskin): calculate something smart based on vel
            # profile from start to end
            # possibly do vel profile based on distance, slow down proportional to
            #  heading error relative to force field

            # rospy.loginfo('gnxt: '+str(force_heading)+' ... '+str(current.theta))

            count += 1
            next_ = current.sample_motion_model2(v, w, dt)

            self.targets.append(next_)

            errors = calc_errors(next_, end)
            along = errors[0]
            # rospy.loginfo('alongher alenghi alphabet: '+str(along))

        self.index = 0



    @unit # forces a unit vector to be returned, scaled by weight
    def get_force_vector(self, start, end, current):
        """
        Aggregate the sum of the force vectors for 3 different actors: departing
        the original location, arriving at the destination and traversing
        between the two points.

        There may be more added, and the end output is
        only relevant for its direction, so the weighting is arbitrary and
        relative, where I've fixed the traverse weight to always be 1.

        This is a superposition of 3? differential equations representing a sum
        of forces.
        """
        wdep = self.weighted_depart(start, end, current)
        warr = self.weighted_arrive(start, end, current)
{'fraction_non_alphanumeric': 0.08395265620699147, 'fraction_numerical': 0.007982383704927057, 'mean_word_length': 2.285262206148282, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, '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': '1987945', 'n_tokens_mistral': 2139, 'n_tokens_neox': 1987, 'n_words': 645}