diff --git a/script/map_updater.py b/script/map_updater.py index 21ee64b..01d6084 100755 --- a/script/map_updater.py +++ b/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() diff --git a/script/predictor.py b/script/predictor.py index 48c5521..acc2ca9 100644 --- a/script/predictor.py +++ b/script/predictor.py @@ -2,58 +2,12 @@ # author: simonlc@stanford.edu +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)