refactoring files to remove unneccessary or duplicate code

lookAhead.py

@@ -1,14 +1,8 @@
 #!/usr/bin/env python
 
-import sys  # for redirecting output in bash, could be removed
-# import time # for sleeping - time.sleep is commented out below right now
-import os
+import sys
 import rospy
-import argparse
-import subprocess
-from subprocess import Popen
 import psutil
-import time
 from geometry_msgs.msg import Twist
 import numpy as np
 import keras
@@ -22,27 +16,14 @@
 v = 0.0
 yaw = 0.0
 
-environments = {0.009: "ice_009",
-                0.09: "ice_09",
-                0.9: "ice_9",
-                1: "control",
-                1000: "mud",
-                0.05: "ice_05",
-                0.5: "ice_5",
-                0.02: "ice_02",
-                0.2: "ice_2",
-                0.07: "ice_07",
-                0.7: "ice_7"}
-
 model = keras.models.load_model('/home/bezzo/catkin_ws/src/capstone_nodes/NNet_all_tf_210.h5', custom_objects={
     'Normalization': keras.layers.experimental.preprocessing.Normalization()})
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -56,51 +37,37 @@ def statesCallback(data):
     yaw = euler[2]
 
 
-def terminate_process_and_children(p):
-    import psutil
-    process = psutil.Process(p.pid)
-    for sub_process in process.children(recursive=True):
-        sub_process.send_signal(signal.SIGINT)
-    # p.wait()  # we wait for children to terminate
-    p.terminate()
-
-
-def signal_process_and_children(pid, signal_to_send, wait=False):
-    process = psutil.Process(pid)
-    for children in process.children(recursive=True):
-        if signal_to_send == 'suspend':
-            children.suspend()
-        elif signal_to_send == 'resume':
-            children.resume()
-        else:
-            children.send_signal(signal_to_send)
-    if wait:
-        process.wait()
-
-
-def terminate_ros_node():
-    list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
-    list_output = list_cmd.stdout.read()
-    retcode = list_cmd.wait()
-    assert retcode == 0, "List command returned %d" % retcode
-    for str in list_output.split("\n"):
-        # if (str.startswith(s)):
-        os.system("rosnode kill " + str)
+def calculate_mu(run):
+    if run <= 120:
+        return 0.009
+    elif 120 < run <= 240:
+        return 0.09
+    elif 240 < run <= 360:
+        return 1
+    elif 360 < run <= 480:
+        return 0.05
+    elif 480 < run <= 600:
+        return 0.5
+    elif 600 < run <= 720:
+        return 0.02
+    elif 720 < run <= 840:
+        return 0.2
+    elif 840 < run <= 960:
+        return 0.07
+    elif 960 < run:
+        return 0.7
+    return None
 
 
 def robotUnsafe(robx, roby, path):
-    # print("hi")
     safety_tolerance = 2
     dists = [0] * len(path)
     i = 0
     for point in path:
         dists[i] = sqrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
-        # print(i)
         i = i + 1
-    # print(dists),
     val = min(dists)
     closest_index = dists.index(val)
-    # print(val)
     return val > safety_tolerance, val, closest_index
 
 
@@ -130,7 +97,6 @@ def getLookAheadPoint(waypoints, robx, roby, lookAheadDistance, lastIndex, lastF
         discriminant = sqrt(discriminant)
         t1 = (-b - discriminant) / (2 * a)
         t2 = (-b + discriminant) / (2 * a)
-        # print('t guys', t1, t2)
         if 0 <= t1 <= 1 and j + t1 > lastFractionalIndex:
             return (E[0] + t1 * d[0], E[1] + t1 * d[1]), j, j + t1
         if 0 <= t2 <= 1 and j + t2 > lastFractionalIndex:
@@ -155,10 +121,8 @@ def injectPoints(waypoints):
         vector = (vector[0] / d * spacing, vector[1] / d * spacing)
         for i in range(0, num_points_that_fit):
             new_list = (start_point[0] + vector[0] * i, start_point[1] + vector[1] * i)
-            # print(new_list)
             new_points.append(new_list)
         new_points.append(end_point)
-    # print(new_points)
     return new_points
 
 
@@ -181,10 +145,10 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
 
 
 def vel_pid(vg, vc, dt):
-    v_goal = vg # output of neural net in m/s
-    v_curr = vc # current velocity in m/s
+    v_goal = vg  # output of neural net in m/s
+    v_curr = vc  # current velocity in m/s
     prev_err = 0.0
-    windup_guard = 10 # needs to be changed??
+    windup_guard = 10  # needs to be changed??
     kp = 1
     ki = 0.1
     kd = 0.1
@@ -196,67 +160,50 @@ def vel_pid(vg, vc, dt):
         i = windup_guard
     elif i > windup_guard:
         i = windup_guard
-    d = 0.0 # use acceleration calculated above??
+    d = 0.0  # use acceleration calculated above??
     if delta_time > 0:
         d = delta_error/dt
     prev_err = error
     vel = p + ki*i + kd*d
     return vel
 
 
-# def main(velocity, angle_deg, log_file, run_num):
-def main(velocity, angle_deg, run_num):
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
+def main(angle_deg):
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
-    # velocity_publisher = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
-    info = "{lin_vel}, {ang_vel}, {angle}, {deviation}\n"
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(angle_deg)  # in radians
     branching_point = (10, 0)
     end_point = (branching_point[0] + 10 * cos(angle), 10 * sin(angle))
     print(end_point)
-    # waypoints = [(10, 0), (0, 10), (10, 10), (0, 0)]
     waypoints = [branching_point, end_point]
     waypoints2 = [(0, 0), branching_point, end_point]
-    path = injectPoints(waypoints2)
     lookAheadDistance = 10
     horizon = 10
     lastIndex = 0
-    # lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
     atGoalHack = 0  # needs to be fixed
     start_angle = yaw
-    # i = 0
-    fut_velocity = 0.5
-
-    # bag_location = "bagfiles/{env}/trainingData".format(env=environments[mu]) + run_num
-    # command = "rosbag record -O " + bag_location + " /gazebo/model_states /odometry/filtered /imu/data"
-    # proc = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, executable='/bin/bash')
 
     while not rospy.is_shutdown():
         path = injectPoints(waypoints2)
         unsafe, robot_deviation, closest_index = robotUnsafe(x, y, path)
         if unsafe:
             print("unsafe")
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
-            # log_file.write(info.format(lin_vel=0, ang_vel=0, angle=angle_deg,
-            #                           deviation=robot_deviation))
+            stop_robot(vel_msg, velocity_publisher)
             break
-        # print(unsafe)
-        # print(robot_deviation)
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
-            # if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1 and atGoalHack>100:
             print("at goal:", x, y)
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
-            # log_file.write(info.format(lin_vel=0, ang_vel=0, angle=angle_deg,
-            #                           deviation=robot_deviation))
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
@@ -265,14 +212,12 @@ def main(velocity, angle_deg, run_num):
         goal_pose_x = lookAheadPoint[0]
         goal_pose_y = lookAheadPoint[1]
 
-        # mu = mu[horizon] ## This is just a general concept for when we don't have a constant mu
         horizon_point1 = path[closest_index + horizon]
         horizon_point2 = path[closest_index + horizon + 1]
         # Estimate angle from starting pose
         angl = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - start_angle
-        # estimate angle from current pose
-        #angl = atan2(horizon_point2[1] - horizon_point1[1], horizon_point2[0] - horizon_point1[0]) - yaw
         fut_velocity = model.predict([[mu, angl]])[0][0]
+
         # linear velocity in the x-axis:
         vel_msg.linear.x = fut_velocity
         vel_msg.linear.y = 0
@@ -288,69 +233,20 @@ def main(velocity, angle_deg, run_num):
         rate.sleep()
         atGoalHack += 1
 
-        # writing to the log file
-        # log_file.write(info.format(lin_vel=vel_msg.linear.x, ang_vel=vel_msg.angular.z,
-        #                           angle=angle_deg, deviation=robot_deviation))
-
-    # log_file.close()
     print("kill me")
     sys.stdout.flush()
     # time.sleep(20)
     raw_input("")  # kill 0 sent from bash script not working, so you have to ctrl-c manually
 
-    # terminate_process_and_children(proc)
-    # signal_process_and_children(proc.pid, signal.SIGINT, True)
-    # terminate_ros_node()
-    # proc.send_signal(subprocess.signal.SIGINT)
-    # proc.kill()
-    # proc.terminate()
     for process in psutil.process_iter():
         print(process.cmdline())
 
 
 if __name__ == "__main__":
     rospy.init_node('capstone_nodes', anonymous=True)
 
-    # get run number
     run = rospy.get_param('~run')
     angle = rospy.get_param('~angle')
-    run_num = str(run)
-
-    # automatically calculate angle
-    # automatically calculate mu
-    mu = 0
-    if run <= 120:
-        mu = 0.009
-    elif 120 < run <= 240:
-        mu = 0.09
-    elif 240 < run <= 360:
-        mu = 1
-    elif 360 < run <= 480:
-        mu = 0.05
-    elif 480 < run <= 600:
-        mu = 0.5
-    elif 600 < run <= 720:
-        mu = 0.02
-    elif 720 < run <= 840:
-        mu = 0.2
-    elif 840 < run <= 960:
-        mu = 0.07
-    elif 960 < run:
-        mu = 0.7
-
-    # use neural network to choose velocity
-
-    # velocity = model.predict([[mu, angle]])[0][0]
-    # velocity = 1.0
-    env = environments[mu]
-
-    # bag_location = "bagfiles/trainingData" + args.run_num
-
-    # log_file = "../logs/{run}_{env}_{vel}_{angle}.txt".format(run=run_num, env=env, vel=str(velocity), angle=str(angle)) # this needs to be fixed, right now you run test.sh from the bagfiles directory
-    # file format: velocity, angle, path_deviation
-    # file = open(log_file, "w")
+    mu = calculate_mu(run)
 
-    print("velocity: ", velocity, "angle: ", angle)
-    # print(angle, args.angle)
-    # main(velocity, angle, file, run_num)
-    main(velocity, angle, run_num)
+    main(angle)

pure_pursuit_cap.py

@@ -5,7 +5,7 @@
 import numpy as np
 from nav_msgs.msg import Odometry
 from gazebo_msgs.msg import ModelStates
-from math import pow, atan2, sqrt, ceil, sin, cos, pi
+from math import pow, atan2, sqrt, ceil, sin, cos, pi, radians
 from tf.transformations import euler_from_quaternion
 
 x = 0.0
@@ -16,10 +16,9 @@
 
 def statesCallback(data):
     global x, y, v, yaw
-    # find index of slash
+    # find index of jackal
     name = data.name
     index = name.index("jackal")
-    # index = name.index("/")
     x = data.pose[index].position.x
     y = data.pose[index].position.y
     v = data.twist[index].linear.x
@@ -38,7 +37,7 @@ def robotUnsafe(robx, roby, path):
     dists = [0]*len(path)
     i = 0
     for point in path:
-        dists[i] = qrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
+        dists[i] = sqrt(pow((point[0] - robx), 2) + pow((point[1] - roby), 2))
     val = min(dists)
     return val > safety_tolerance
 
@@ -117,37 +116,36 @@ def smoothPath(path):  # path is [(x1, y1), ..., (xend, yend)]
     return newPath
 
 
+def stop_robot(vel_msg, velocity_publisher):
+    vel_msg.linear.x = 0
+    vel_msg.angular.z = 0
+    velocity_publisher.publish(vel_msg)
+
+
 def main():
     rospy.init_node('pure_pursuit_cap', anonymous=True)
     velocity_publisher = rospy.Publisher('/jackal_velocity_controller/cmd_vel', Twist, queue_size=10)
-    # velocity_publisher = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
     rospy.Subscriber('/gazebo/model_states', ModelStates, statesCallback)
     rate = rospy.Rate(10)
     vel_msg = Twist()
     angle = radians(20)
     branching_point = (10, 0)
     end_point = (branching_point[0] + 10*cos(angle), 10*sin(angle))
-    #waypoints = [(10, 0), (0, 10), (10, 10), (0, 0)]
     waypoints = [branching_point, end_point]
     path = injectPoints(waypoints)
     lookAheadDistance = 2
     lastIndex = 0
-    lastLookAheadIndex = 0
     lastFractionalIndex = 0
     lookAheadPoint = waypoints[0]
-    i = 0
+
     while not rospy.is_shutdown():
 
         if robotUnsafe (x, y, path):
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         if robotAtGoal(x, y, waypoints[-1][0], waypoints[-1][1]) and lastIndex == len(waypoints) - 1:
-            vel_msg.linear.x = 0
-            vel_msg.angular.z = 0
-            velocity_publisher.publish(vel_msg)
+            stop_robot(vel_msg, velocity_publisher)
             break
 
         lookAheadPoint, lastIndex, lastFractionalIndex = getLookAheadPoint(waypoints, x, y, lookAheadDistance,
@@ -173,4 +171,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()