address PR comments

- conversion between CARLA left-handed coordinates and ROS right-handed
coordinates. Do the conversion while reading in csv file and keep all
internal code to be right-handed coordinates.
- remove redundant re-definition of odom_state classes
- clean up and add documentation

launch/car_simulation.launch

@@ -2,7 +2,7 @@
 
 <launch>
 
-	<arg name="rolename" default="stanford_ego" />
+	<arg name="rolename" default="hero" />
 	<arg name="track_steps" default="300" />
 	<arg name="distance" default="4.0" />
 	<arg name="color" default="255, 200, 200" />
@@ -19,6 +19,7 @@
 		<!-- Mapper node - publish local track
 			center and track width for two cars -->
 		<node name="map_updater" pkg="carla_circle" type="map_updater.py" output="screen">
+			<param name="rolename" value="$(arg rolename)" />
 			<param name="steps" value="$(arg track_steps)" />
 			<param name="update_frequency" value="10.0" />
 		</node>

launch/mcity_event1.launch

@@ -17,9 +17,6 @@
 		<arg name="fixed_delta_seconds" value="false" />
 	</include>
 
-	<!-- shared by all the cars  -->
-	<!-- <node name="car_filter" pkg="carla_circle" type="car_filter.py" output="screen" /> -->
-
 	<include file="$(find carla_circle)/launch/car_simulation.launch">
 		<arg name="rolename" value="$(arg rolename)" />
 		<arg name="color" value="255, 0, 0" />

script/controller.py

@@ -12,96 +12,7 @@
 from scipy.spatial import KDTree
 import numpy as np
 
-
-class odom_state(object):
-    '''
-    This class stores an instantaneous odometry state of the vehicle. All units
-    are SI.
-    '''
-    def __init__(self):
-        '''
-        Initializes the class variables.
-        '''
-        rospy.loginfo_once("Initializing odometry state.")
-        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):
-        '''
-        Updates the member variables to the instantaneous odometry state of a
-        vehicle.
-
-        Parameters
-        ----------
-        odom_msg : Odometry
-            A message containing the current odometry state of the vehicle.
-        '''
-        self.time = odom_msg.header.stamp.to_sec()
-        self.x = odom_msg.pose.pose.position.x
-        self.y = odom_msg.pose.pose.position.y
-        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
-        )
-        ori_euler = tf.transformations.euler_from_quaternion(ori_quat)
-        self.yaw = 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):
-        '''
-        Returns the stored global position :math:`(x, y)` of the vehicle
-        center.
-
-        Returns
-        -------
-        Array
-            The 2D position (global) of the vehicle.
-        '''
-        return [self.x, self.y]
-
-    def get_pose(self):
-        '''
-        Returns the stored global position :math:`(x, y)` of the vehicle center
-        and heading/yaw :math:`(\theta)` with respect to the x-axis.
-
-        Returns
-        -------
-        Array
-            A SE(2) vector representing the global position and heading of the
-            vehicle.
-        '''
-        return [self.x, self.y, self.yaw]
-
-    def get_velocity(self):
-        '''
-        Returns the stored global velocity :math:`(v_x, v_y)` of the vehicle.
-
-        Returns
-        -------
-        Array
-            The 2D velocity (global) of the vehicle.
-        '''
-        return [self.vx, self.vy]
-
-    def get_speed(self):
-        '''
-        Returns the stored global speed of the vehicle.
-
-        Returns
-        -------
-        float
-            The speed (global) of the vehicle.
-        '''
-        return self.speed
+from map_updater import odom_state
 
 
 class AckermannController:

script/map_updater.py

@@ -28,6 +28,10 @@ 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.
+
     Parameters
     ----------
     filename : str
@@ -50,7 +54,7 @@ def get_mcity_route(filename):
         prev = [-99999.0, -99999.0]
 
         for _, element in enumerate(data):
-            waypoint = [element[1], element[2]]
+            waypoint = [element[1], -element[2]]
 
             if not np.allclose(waypoint, prev):
                 waypoints.append(waypoint)
@@ -184,6 +188,7 @@ def __init__(self):
         # retrieve ros parameters
         self.steps = rospy.get_param("~steps")  # Num of waypoints in the track
         freq = rospy.get_param("~update_frequency")
+        role_name = rospy.get_param("~rolename")
 
         # state information
         self.stateReady = False
@@ -199,7 +204,7 @@ def __init__(self):
 
         # Subscribers for ego and opponent vehicle odometry
         rospy.Subscriber(
-            "/carla/hero/odometry",
+            "/carla/" + role_name + "/odometry",
             Odometry,
             self.odom_cb
         )
@@ -252,7 +257,7 @@ def publish_global_path(self):
             pose_s = PoseStamped()
             pose_s.header = global_path.header
             pose_s.pose.position.x = data[0]
-            pose_s.pose.position.y = -data[1]
+            pose_s.pose.position.y = data[1]
             global_path.poses.append(pose_s)
 
         self.global_plan_pub.publish(global_path)
@@ -288,7 +293,6 @@ def ado_odom_cb(self, msg):
             self.ado_stateReady = True
 
     def get_path_from_position(self, position_x, position_y):
-
         '''
         Returns the track width and center based on the position of the
         vehicle.
@@ -308,12 +312,12 @@ def get_path_from_position(self, position_x, position_y):
         track_center = np.zeros((2, self.steps))
         track_width = 3.0
 
-        _, idx = self.global_path.query([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()
         else:
             track_center = self.global_path.data[idx - 1: idx + self.steps - 1, :].T.copy()
-        track_center[1,:] = -track_center[1,:]
+        track_center[1,:] = track_center[1,:]
 
         rospy.logdebug(
             "Track:\nCenters - {}\nWidth - {}".format(

script/planner.py

@@ -13,58 +13,7 @@
 import numpy as np
 import math
 
-
-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
-
-        self.circle_center = circle_center
-        self.circle_radius = circle_radius
-        self.circle_angle = 0      # this is in polar coordinate
-
-    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
-        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)
-        ori_euler = tf.transformations.euler_from_quaternion(ori_quat)
-        self.yaw = 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)
-        self.circle_angle = np.arctan2(
-            self.y - self.circle_center[1],
-            self.x - self.circle_center[0]
-        )
-
-    def get_polar_angle(self):
-        return self.circle_angle
-
-    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 map_updater import odom_state
 
 
 class TrajectoryPlanner:

script/trivial_planner.py

@@ -12,51 +12,16 @@
 import numpy as np
 import math
 
-
-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
-        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)
-        ori_euler = tf.transformations.euler_from_quaternion(ori_quat)
-        self.yaw = 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 map_updater import odom_state
 
 
 class TrivialPlanner:
+    '''
+    A ROS node used to publish a trajectory for ego vehicle.
+
+    Minimum planning task: takes in a path (possibly lane center), and interpolates it based
+    on current velocity to get a trajectory,
+    '''
     MAX_ACCELERATION = 3
     MAX_ACCELERATION_ANG = 3 / 19.5
 
@@ -73,18 +38,9 @@ def __init__(self):
         # class attributes
         self.track = Path()  # predicted trajs for relevant cars
         self.track_ready = False
-        # predicted _trajectories
-        # 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
-
-        # self.circle_radius = 19.5  # radius of the circle trajectory
-        # self.circle_center = [-0.5, -0.3]  # center offset of the circle traj
 
         self.stateReady = False
-        self.state = odom_state(0, 0)
+        self.state = odom_state()
 
         # Subscribers for ego vehicle odometry and predicted trajectory
         rospy.Subscriber(

script/visualization_node.py

@@ -5,7 +5,6 @@
 from visualization_classes import egoTrackVisualizer
 from visualization_classes import oppTrackVisualizer
 from visualization_classes import PredictWaypointsVisualization
-# from visualization_classes import RoadGeometryVisualization
 from visualization_classes import RoadNetworkVisualization
 
 import carla
@@ -25,11 +24,9 @@
         carla_world = carla_client.get_world()
 
         waypointsviz = DesiredWaypointsVisualization()
-        # road_viz = RoadGeometryVisualization()
         road_topo_viz = RoadNetworkVisualization(carla_world)
         ego_track_viz = egoTrackVisualizer()
         opp_track_viz = oppTrackVisualizer()
-        # pre_waypoints_viz = PredictWaypointsVisualization()
 
         rospy.spin()