Issue #3: Updates some formatting

- Updates the formatting in map_updater and predictor nodes to match PEP 8 standard.

script/map_updater.py

@@ -5,18 +5,15 @@
 
 import rospy
 import tf
-from geometry_msgs.msg import Pose, PoseStamped
+from geometry_msgs.msg import PoseStamped
 from nav_msgs.msg import Path, Odometry
 
-
 import numpy as np
 from numpy import genfromtxt
 from scipy.spatial import KDTree
 
 import os
-import random
 
-import carla
 
 def get_mcity_route(filename):
     '''
@@ -28,9 +25,9 @@ def get_mcity_route(filename):
     obtained from a recording of Michigan's vehicle, the csv has several
     waypoints consecutively from where the vehicle is not moving.
 
-    Note: csv file data is given in lefthanded coordinates, while ROS and all our
-    code use righthanded coordinates. Thus, we flip the sign of y coordintes while
-    reading in waypoints.
+    Note: csv file data is given in lefthanded coordinates, while ROS and all
+    our code use righthanded coordinates. Thus, we flip the sign of y
+    coordinates whilereading in waypoints.
 
     Parameters
     ----------
@@ -201,7 +198,6 @@ def __init__(self):
         self.ado_track_info = Path()
         self.global_path = get_mcity_route('ego_trajectory_event1.csv')
 
-
         # Subscribers for ego and opponent vehicle odometry
         rospy.Subscriber(
             "/carla/" + role_name + "/odometry",
@@ -314,10 +310,14 @@ def get_path_from_position(self, position_x, position_y):
 
         _, idx = self.global_path.query([position_x, position_y])
         if idx < 1:
-            track_center = self.global_path.data[idx: idx + self.steps, :].T.copy()
+            track_center = self.global_path.data[
+                idx: idx + self.steps, :
+            ].T.copy()
         else:
-            track_center = self.global_path.data[idx - 1: idx + self.steps - 1, :].T.copy()
-        track_center[1,:] = track_center[1,:]
+            track_center = self.global_path.data[
+                idx - 1: idx + self.steps - 1, :
+            ].T.copy()
+        track_center[1, :] = track_center[1, :]
 
         rospy.logdebug(
             "Track:\nCenters - {}\nWidth - {}".format(
@@ -401,6 +401,7 @@ def timer_cb(self, event):
 
         self.publish_global_path()
 
+
 if __name__ == '__main__':
     try:
         supervisor = MapUpdater()

script/predictor.py

@@ -2,58 +2,12 @@
 
 # author: [email protected]
 
+from map_updater import odom_state
+
 import rospy
-import tf
 from nav_msgs.msg import Odometry, Path, PoseStamped, Pose
-from carla_circle.msg import PathArray
-from geometry_msgs.msg import Transform, Vector3, Quaternion, Twist, Point
-
-import numpy as np
-import math
-import timeit
-
-
-class odom_state(object):
-    def __init__(self, circle_center, circle_radius):
-        self.time = None
-        self.x = None
-        self.y = None
-        self.yaw = None
-        self.vx = None
-        self.vy = None
-        self.speed = None
-
-    def update_vehicle_state(self, odom_msg):
-        '''
-        input: ROS Odometry message
-        output: None
-        updates the time instance, position, orientation, velocity and speed
-        '''
-        self.time = odom_msg.header.stamp.to_sec()
-        self.x = odom_msg.pose.pose.position.x
-        self.y = odom_msg.pose.pose.position.y
-        self.z = odom_msg.pose.pose.position.z
-        self.ori_quat = (odom_msg.pose.pose.orientation.x,
-                    odom_msg.pose.pose.orientation.y,
-                    odom_msg.pose.pose.orientation.z,
-                    odom_msg.pose.pose.orientation.w)
-        self.ori_euler = tf.transformations.euler_from_quaternion(self.ori_quat)
-        self.yaw = self.ori_euler[2]
-        self.vx = odom_msg.twist.twist.linear.x
-        self.vy = odom_msg.twist.twist.linear.y
-        self.speed = np.sqrt(self.vx**2 + self.vy**2)
-
-    def get_position(self):
-        return [self.x, self.y]
-
-    def get_pose(self):
-        return [self.x, self.y, self.yaw]
-
-    def get_velocity(self):
-        return [self.vs, self.vy]
-
-    def get_speed(self):
-        return self.speed
+from geometry_msgs.msg import Point
+
 
 class TrajectoryPredictor:
     def __init__(self):
@@ -65,23 +19,32 @@ def __init__(self):
         self.steps = int(self.horizon / self.time_step)
 
         # class attributes
-        self.predicted_traj = Path()     # predicted trajectories for relevant cars
+        self.predicted_traj = Path()
         # predicted _trajectories
+        # predicted trajectories for relevant objects (vehicles, walkers, etc.)
         # each path in PathArray.paths has self.steps poses
         # the first pose start from the current location of the relevant car
         # the distance between sequential poses is speed(car) * self.time_step
-        # this information is used to infer the possible time when the relevant cars
-        # enter the roundabout
+        # this information is used to infer the possible time when the relevant
+        # cars enter the roundabout
 
         self.state = odom_state()
         self.stateReady = False
 
         # subscribers, publishers
-        rospy.Subscriber("/carla/vehicle/<ID>/odometry", Odometry, self.odom_cb) # need to deal with <ID>
-        self.trajectory_pub = rospy.Publisher("/carla/vehicle/<ID>/predicted_traj", \ # need to deal with <ID>
-                                Path, queue_size=10)
-        self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb)
+        rospy.Subscriber(
+            "/carla/vehicle/<ID>/odometry",
+            Odometry,
+            self.odom_cb
+        )  # need to deal with <ID>
+
+        self.trajectory_pub = rospy.Publisher(
+            "/carla/vehicle/<ID>/predicted_traj",
+            Path,
+            queue_size=10
+        )  # need to deal with <ID>
 
+        self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb)
 
     '''
     this function updates the ego vehicle state in class
@@ -100,19 +63,19 @@ def timer_cb(self, event):
             traj_msg.header.stamp = rospy.Time.now()
             traj_msg.header.frame_id = 'map'
             traj_msg.poses = []
-            for k in range(sefl.steps+1):
+            for k in range(self.steps+1):
                 # Fill position and velocity at time step k in the traj_msg
                 dt = k*self.time_step
                 xk = pos_x + dt*velocity[0]
                 yk = pos_y + dt*velocity[1]
 
                 pose_stamped = PoseStamped()
-                pose_stamped.header.stamp = traj_msg.header.stamp + dt # not sure the time units are the same here
+                pose_stamped.header.stamp = traj_msg.header.stamp + dt  # not sure the time units are the same here
                 pose_stamped.header.frame_id = 'map'
                 pose_stamped.pose = Pose(
                     Point(xk, yk, self.z),
-                    sefl.ori_quat,
-                    )
+                    self.ori_quat,
+                )
 
                 traj_msg.poses.append(pose_stamped)