From 43c5a5d83b2000763eb72a0c223664161a7b6df3 Mon Sep 17 00:00:00 2001 From: mingyu wang Date: Sun, 1 Nov 2020 15:45:34 -0800 Subject: [PATCH 1/5] remove code from game planner - remove uav_game planner node - remove topic name convention from game-planner --- launch/car_simulation.launch | 35 ++++++++++++--------------------- launch/mcity_event1.launch | 11 +---------- script/controller.py | 9 +++++---- script/visualization_classes.py | 24 +++++++++++----------- script/visualization_node.py | 6 +++--- 5 files changed, 34 insertions(+), 51 deletions(-) diff --git a/launch/car_simulation.launch b/launch/car_simulation.launch index 30946ce..98e7b9c 100644 --- a/launch/car_simulation.launch +++ b/launch/car_simulation.launch @@ -3,44 +3,35 @@ - + - + - + - - - - - - - - - - + - - - - - - - - + + + + + + + @@ -71,4 +62,4 @@ - \ No newline at end of file + diff --git a/launch/mcity_event1.launch b/launch/mcity_event1.launch index 54d1e19..5d358d7 100644 --- a/launch/mcity_event1.launch +++ b/launch/mcity_event1.launch @@ -18,7 +18,7 @@ - + @@ -33,13 +33,4 @@ - - - - - - - - - diff --git a/script/controller.py b/script/controller.py index b188539..2a52459 100755 --- a/script/controller.py +++ b/script/controller.py @@ -152,7 +152,7 @@ def __init__(self): self.odom_cb ) rospy.Subscriber( - "MSLcar0/command/trajectory", + "ego/command/trajectory", MultiDOFJointTrajectory, self.desired_waypoints_cb ) @@ -171,9 +171,10 @@ def __init__(self): topic = "/carla/{}/vehicle_info".format(rolename) rospy.loginfo_once( - "Vehicle information for %s: %s", - rolename, - rospy.wait_for_message(topic, CarlaEgoVehicleInfo) + "Vehicle information for {}: {}".format( + rolename, + rospy.wait_for_message(topic, CarlaEgoVehicleInfo) + ) ) # Class timer diff --git a/script/visualization_classes.py b/script/visualization_classes.py index ddc5abb..6f3c22d 100644 --- a/script/visualization_classes.py +++ b/script/visualization_classes.py @@ -26,7 +26,7 @@ def __init__(self): vehicle used throughout the class. ''' rospy.Subscriber( - "MSLcar0/command/trajectory", + "ego/command/trajectory", MultiDOFJointTrajectory, self.des_traj_cb ) @@ -45,9 +45,9 @@ def des_traj_cb(self, msg): mk.header.stamp = rospy.Time.now() mk.header.frame_id = "map" mk.type = Marker.CUBE_LIST - mk.scale.x = 1.3 - mk.scale.y = 1.3 - mk.scale.z = 1.3 + mk.scale.x = 0.5 + mk.scale.y = 0.5 + mk.scale.z = 0.5 mk.color.a = 1.0 mk.color.g = 1 @@ -116,7 +116,7 @@ def __init__(self): vehicle used throughout the class. ''' rospy.Subscriber( - "MSLcar0/mpc/ego_track_information", + "ego_track_information", Path, self.ego_track_cb ) @@ -135,9 +135,9 @@ def ego_track_cb(self, msg): for i in range(len(msg.poses)-2): pos_x = msg.poses[i].pose.position.x - pos_y = -msg.poses[i].pose.position.y + pos_y = msg.poses[i].pose.position.y pos_x_n = msg.poses[i+1].pose.position.x - pos_y_n = -msg.poses[i+1].pose.position.y + pos_y_n = msg.poses[i+1].pose.position.y yaw = np.arctan2(pos_y_n - pos_y, pos_x_n - pos_x) mk = Marker() @@ -145,10 +145,10 @@ def ego_track_cb(self, msg): mk.header.stamp = rospy.Time.now() mk.header.frame_id = "map" mk.type = Marker.CUBE - mk.scale.x = 4 - mk.scale.y = msg.poses[-1].pose.position.x * 2 - mk.scale.z = 1.3 - mk.color.a = 0.2 + mk.scale.x = 0.1 + mk.scale.y = msg.poses[-1].pose.position.x * 2 # the last one is used for track width + mk.scale.z = 0.3 + mk.color.a = 0.1 mk.color.g = 255 mk.pose.position.x = pos_x mk.pose.position.y = pos_y @@ -172,7 +172,7 @@ def __init__(self): vehicle used throughout the class. ''' rospy.Subscriber( - "MSLcar0/mpc/ado_track_information", + "ado_track_information", Path, self.track_cb ) diff --git a/script/visualization_node.py b/script/visualization_node.py index 168978b..cb32dec 100755 --- a/script/visualization_node.py +++ b/script/visualization_node.py @@ -5,7 +5,7 @@ from visualization_classes import egoTrackVisualizer from visualization_classes import oppTrackVisualizer from visualization_classes import PredictWaypointsVisualization -from visualization_classes import RoadGeometryVisualization +# from visualization_classes import RoadGeometryVisualization from visualization_classes import RoadNetworkVisualization import carla @@ -25,11 +25,11 @@ carla_world = carla_client.get_world() waypointsviz = DesiredWaypointsVisualization() - road_viz = RoadGeometryVisualization() + # road_viz = RoadGeometryVisualization() road_topo_viz = RoadNetworkVisualization(carla_world) ego_track_viz = egoTrackVisualizer() opp_track_viz = oppTrackVisualizer() - pre_waypoints_viz = PredictWaypointsVisualization() + # pre_waypoints_viz = PredictWaypointsVisualization() rospy.spin() From 91164a89354e5beba7729ee27b48f1a3aa11d566 Mon Sep 17 00:00:00 2001 From: mingyu wang Date: Sun, 1 Nov 2020 15:48:15 -0800 Subject: [PATCH 2/5] bug fix in map_updater carla uses left hand coordinate but ros uses right hand coordinate need to flip signs of y coordinate given in the csv file --- script/map_updater.py | 66 ++++++++++++------------------------------- 1 file changed, 18 insertions(+), 48 deletions(-) diff --git a/script/map_updater.py b/script/map_updater.py index 7c1dce2..8a912be 100755 --- a/script/map_updater.py +++ b/script/map_updater.py @@ -17,8 +17,6 @@ import random import carla -from carla_circle.srv import GetAvailablePath - def get_mcity_route(filename): ''' @@ -28,7 +26,7 @@ def get_mcity_route(filename): The parsing of the waypoints is done in such a way to try and remove consecutive duplicates from the csv file. Since these files seem to be the obtained from a recording of Michigan's vehicle, the csv has several - waypoints consecutively from where the vehicle is not moving. + waypoints consecutively from where the vehicle is not moving. Parameters ---------- @@ -168,11 +166,9 @@ def get_speed(self): class MapUpdater: ''' - A ROS node used to publish the drivable track center given current - position of a car. + A ROS node used to publish the near-future route of ego vehicle. - The node updates at a provided frequency. This class is designed to work - with an optimization based game-theoretic planner. + The node publishes at a provided frequency. ''' def __init__(self): ''' @@ -185,20 +181,9 @@ def __init__(self): "Initializing map updater for namespace '{}'".format(ns) ) - # Get Carla server information - host = rospy.get_param("/carla/host") - port = rospy.get_param("/carla/port") - carla_client = carla.Client(host, port) - carla_world = carla_client.get_world() - self.carla_map = carla_world.get_map() - # retrieve ros parameters self.steps = rospy.get_param("~steps") # Num of waypoints in the track - self.distance = rospy.get_param("~distance") # dist btw 2 waypoints freq = rospy.get_param("~update_frequency") - self.max_speed = rospy.get_param("~max_speed") - self.opp_speed = rospy.get_param("~opp_speed") - self.plan_horizon = rospy.get_param("~plan_horizon") # state information self.stateReady = False @@ -206,25 +191,20 @@ def __init__(self): self.ado_stateReady = False self.ado_state = odom_state() - self.current_stage = 1 # 1 -- start and circle 2 -- existing - # path information self.ego_track_info = Path() self.ado_track_info = Path() self.global_path = get_mcity_route('ego_trajectory_event1.csv') - # service proxy to get a path update from carla world - rospy.wait_for_service("get_path", 8.0) - self.get_path_handle = rospy.ServiceProxy('get_path', GetAvailablePath) # Subscribers for ego and opponent vehicle odometry rospy.Subscriber( - "MSLcar0/ground_truth/odometry", + "/carla/hero/odometry", Odometry, self.odom_cb ) rospy.Subscriber( - "MSLcar1/ground_truth/odometry", + "ado/ground_truth/odometry", Odometry, self.ado_odom_cb ) @@ -232,12 +212,12 @@ def __init__(self): # Publishers for track information of ego and opponent, as well as # global route self.ego_track_pub = rospy.Publisher( - "MSLcar0/mpc/ego_track_information", + "ego_track_information", Path, queue_size=10 ) self.ado_track_pub = rospy.Publisher( - "MSLcar0/mpc/ado_track_information", + "ado_track_information", Path, queue_size=10 ) @@ -308,6 +288,7 @@ 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. @@ -326,27 +307,14 @@ def get_path_from_position(self, position_x, position_y): ''' track_center = np.zeros((2, self.steps)) track_width = 3.0 - waypoint = self.carla_map.get_waypoint( - carla.Location(position_x, position_y, 0.0) - ) - # if waypoint.lane_change is carla.LaneChange.Right: - # track_width = 2.0 * waypoint.lane_width - # elif waypoint.lane_change is carla.LaneChange.Left: - # track_width = 2.0 * waypoint.lane_width - # elif waypoint.lane_change is carla.LaneChange.Both: - # track_width = 3.0 * waypoint.lane_width - # else: - # track_width = waypoint.lane_width - track_width = waypoint.lane_width - - _, idx = self.global_path.query([position_x, position_y]) - if idx < 2: - track_center = self.global_path.data[idx: idx + 20, :].T.copy() + _, 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 - 2: idx + 18, :].T.copy() + track_center = self.global_path.data[idx - 1: idx + self.steps - 1, :].T.copy() + track_center[1,:] = -track_center[1,:] - # rospy.logdebug( rospy.logdebug( "Track:\nCenters - {}\nWidth - {}".format( track_center, track_width @@ -419,13 +387,15 @@ def timer_cb(self, event): event : rospy.TimerEvent The timer's tick event. ''' - if self.stateReady and self.ado_stateReady: + if self.stateReady: self.update_ego_track() - self.update_ado_track() - self.ego_track_pub.publish(self.ego_track_info) + + if self.ado_stateReady: + self.update_ado_track() self.ado_track_pub.publish(self.ado_track_info) + self.publish_global_path() if __name__ == '__main__': try: From 47015a27fea4d642a8173f5cf5585c3b8a6b0594 Mon Sep 17 00:00:00 2001 From: mingyu wang Date: Sun, 1 Nov 2020 15:50:25 -0800 Subject: [PATCH 3/5] a trivial planner as a place holder for planner module - takes in path from map_updater - publishes a trajectory assuming constant reference speed --- script/trivial_planner.py | 191 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100755 script/trivial_planner.py diff --git a/script/trivial_planner.py b/script/trivial_planner.py new file mode 100755 index 0000000..12a8fd0 --- /dev/null +++ b/script/trivial_planner.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python + +# author: mingyuw@stanford.edu + +import rospy +import tf +from nav_msgs.msg import Odometry, Path +from trajectory_msgs.msg import MultiDOFJointTrajectory +from trajectory_msgs.msg import MultiDOFJointTrajectoryPoint +from geometry_msgs.msg import Transform, Vector3, Quaternion, Twist + +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 + + +class TrivialPlanner: + MAX_ACCELERATION = 3 + MAX_ACCELERATION_ANG = 3 / 19.5 + + def __init__(self): + rospy.init_node("planner", anonymous=True) + + # retrieve ros parameters + rolename = rospy.get_param("~rolename") + self.time_step = rospy.get_param("~time_step") + self.horizon = rospy.get_param("~horizon") + self.steps = int(self.horizon / self.time_step) + self.reference_speed = rospy.get_param("~max_speed") + + # 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) + + # Subscribers for ego vehicle odometry and predicted trajectory + rospy.Subscriber( + "/carla/" + rolename + "/odometry", + Odometry, + self.odom_cb + ) + rospy.Subscriber( + "ego_track_information", + Path, + self.track_cb + ) + + # Publisher for desired waypoints + self.trajectory_pub = rospy.Publisher( + "ego/command/trajectory", + MultiDOFJointTrajectory, + queue_size=10 + ) + + # Main timer for planner + self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb) + + ''' + this function updates the ego vehicle state in class + ''' + def odom_cb(self, msg): + self.state.update_vehicle_state(msg) + if not self.stateReady: + self.stateReady = True + + def track_cb(self, msg): + self.track = msg + if not self.track_ready: + self.track_ready = True + + def get_track_info(self): + num_track_points = len(self.track.poses) - 1 + track_center = np.zeros((2, num_track_points)) + track_length = np.zeros((num_track_points,)) + track_dir = np.zeros((2,num_track_points)) + track_width = self.track.poses[num_track_points].pose.position.x + for i in range(num_track_points): + track_center[0, i] = self.track.poses[i].pose.position.x + track_center[1, i] = self.track.poses[i].pose.position.y + if i == 0: + track_length[i] = 0 + else: + track_length[i] = track_length[i-1] + np.linalg.norm(track_center[:,i] - track_center[:,i-1]) + for i in range(num_track_points): + if i < num_track_points - 1: + track_dir[:,i] = (track_center[:,i+1] - track_center[:,i])/np.linalg.norm(track_center[:,i+1] - track_center[:,i]) + else: + track_dir[:,i] = track_dir[:,i-1] + + return track_center, track_width, track_dir, track_length + + def timer_cb(self, event): + # interpolate path using max speed or current speed + if self.stateReady and self.track_ready: + pos_x, pos_y = self.state.get_position() + speed = self.reference_speed + + track_center, track_width, track_dir, track_length = self.get_track_info() + # use interpolation, always assume ego vehicle strictly inside the + interpolate_target_distance = np.array([i * self.reference_speed * self.time_step for i in range(self.steps)]) + initial_offset = np.dot([pos_x, pos_y] - track_center[:,0], track_dir[:,0]) # distance ego ahead of track start point + track_length = track_length - initial_offset + + desired_trajectory = np.zeros((2, self.steps)) + desired_trajectory[0,:] = np.interp(interpolate_target_distance, track_length, track_center[0,:]) + desired_trajectory[1,:] = np.interp(interpolate_target_distance, track_length, track_center[1,:]) + traj_msg = MultiDOFJointTrajectory() + traj_msg.header.stamp = rospy.Time.now() + traj_msg.header.frame_id = 'map' + traj_msg.points = [] + for i in range(self.steps): + traj_point = MultiDOFJointTrajectoryPoint() + traj_point.transforms = [ + Transform( + Vector3( + desired_trajectory[0,i], + desired_trajectory[1,i], 0 + ), + Quaternion(0, 0, 0, 1) + ) + ] + traj_point.velocities = [ + Twist( + Vector3(self.reference_speed, 0, 0), + Vector3(0, 0, 0) + ) + ] + traj_msg.points.append(traj_point) + + self.trajectory_pub.publish(traj_msg) + + +if __name__ == '__main__': + try: + planner = TrivialPlanner() + rospy.spin() + except rospy.ROSInterruptException: + pass From c28912f0c1f6b7f937404db7ade8df12c3811c0d Mon Sep 17 00:00:00 2001 From: mingyu wang Date: Sun, 1 Nov 2020 15:58:11 -0800 Subject: [PATCH 4/5] update rviz file - set default to use .rviz file from carla_circle package --- launch/mcity_event1.launch | 2 +- resource/carla_default_rviz.cfg.rviz | 260 +++++++++++++++++++++++++++ 2 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 resource/carla_default_rviz.cfg.rviz diff --git a/launch/mcity_event1.launch b/launch/mcity_event1.launch index 5d358d7..bd602dc 100644 --- a/launch/mcity_event1.launch +++ b/launch/mcity_event1.launch @@ -31,6 +31,6 @@ - + diff --git a/resource/carla_default_rviz.cfg.rviz b/resource/carla_default_rviz.cfg.rviz new file mode 100644 index 0000000..0452950 --- /dev/null +++ b/resource/carla_default_rviz.cfg.rviz @@ -0,0 +1,260 @@ +Panels: + - Class: rviz/Displays + Help Height: 75 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Ego trajectory1 + Splitter Ratio: 0.6000000238418579 + Tree Height: 366 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + - /ThirdPersonFollower1 + - /ThirdPersonFollower2 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: rviz/Displays + Help Height: 75 + Name: Displays + Property Tree Widget: + Expanded: + - /EgoOdometry1/Shape1 + - /Origin1 + Splitter Ratio: 0.44411763548851013 + Tree Height: 481 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: EgoOdometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 1 + Head Radius: 1 + Shaft Length: 3 + Shaft Radius: 1 + Value: Arrow + Topic: /carla/hero/odometry + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /hero/carla/viz/des_waypoints + Name: Ego trajectory + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /hero/carla/viz/ego_track + Name: Ego Track + Namespaces: + "": true + Queue Size: 100 + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Global Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /hero/global_plan + Unreliable: false + Value: true + - Class: rviz/Axes + Enabled: true + Length: 10 + Name: Origin + Radius: 0.5 + Reference Frame: map + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /hero/carla/viz/road_topo + Name: Road Topology + Namespaces: + "": true + Queue Size: 100 + Value: true + - Alpha: 0.5 + Cell Size: 10 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + Enabled: true + Global Options: + Background Color: 80; 80; 80 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 68.55451965332031 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6602022647857666 + Target Frame: hero + Value: ThirdPersonFollower (rviz) + Yaw: 4.739986896514893 + Saved: + - Class: rviz/ThirdPersonFollower + Distance: 200 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: hero + Value: ThirdPersonFollower (rviz) + Yaw: 3.140000104904175 + - Class: rviz/ThirdPersonFollower + Distance: 50 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Target Frame: hero + Value: ThirdPersonFollower (rviz) + Yaw: 3.140000104904175 +Window Geometry: + Displays: + collapsed: false + Height: 808 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ab00000259fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000000d7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003d000000e80000000000000000fb0000000a0049006d006100670065010000003d000002590000000000000000fb0000000a0049006d00610067006501000001680000012e0000000000000000fb0000000a0049006d00610067006501000002b800000131000000000000000000000001000001fb00000286fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d00000286000000e60100001cfa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb0000000a00560069006500770073010000063a0000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054b00000042fc0100000002fb0000000800540069006d006501000000000000054b000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000034a0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1355 + X: 204 + Y: 27 From 6514008bab418894590570e9d60ec109f7780958 Mon Sep 17 00:00:00 2001 From: mingyu wang Date: Fri, 13 Nov 2020 17:44:51 -0800 Subject: [PATCH 5/5] 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 | 3 +- launch/mcity_event1.launch | 3 -- script/controller.py | 91 +----------------------------------- script/map_updater.py | 16 ++++--- script/planner.py | 53 +-------------------- script/trivial_planner.py | 60 ++++-------------------- script/visualization_node.py | 3 -- 7 files changed, 22 insertions(+), 207 deletions(-) diff --git a/launch/car_simulation.launch b/launch/car_simulation.launch index 98e7b9c..d2ba46a 100644 --- a/launch/car_simulation.launch +++ b/launch/car_simulation.launch @@ -2,7 +2,7 @@ - + @@ -19,6 +19,7 @@ + diff --git a/launch/mcity_event1.launch b/launch/mcity_event1.launch index bd602dc..4a892ac 100644 --- a/launch/mcity_event1.launch +++ b/launch/mcity_event1.launch @@ -17,9 +17,6 @@ - - - diff --git a/script/controller.py b/script/controller.py index 2a52459..faad2d1 100755 --- a/script/controller.py +++ b/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: diff --git a/script/map_updater.py b/script/map_updater.py index 8a912be..21ee64b 100755 --- a/script/map_updater.py +++ b/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( diff --git a/script/planner.py b/script/planner.py index 66a20a6..01380b7 100755 --- a/script/planner.py +++ b/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: diff --git a/script/trivial_planner.py b/script/trivial_planner.py index 12a8fd0..2a04ce6 100755 --- a/script/trivial_planner.py +++ b/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( diff --git a/script/visualization_node.py b/script/visualization_node.py index cb32dec..49f54cf 100755 --- a/script/visualization_node.py +++ b/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()