diff --git a/launch/car_simulation.launch b/launch/car_simulation.launch index 30946ce..d2ba46a 100644 --- a/launch/car_simulation.launch +++ b/launch/car_simulation.launch @@ -2,45 +2,37 @@ - - + + - + - + - - - - - - - - - - + + - - - - - - - - + + + + + + + @@ -71,4 +63,4 @@ - \ No newline at end of file + diff --git a/launch/mcity_event1.launch b/launch/mcity_event1.launch index 54d1e19..4a892ac 100644 --- a/launch/mcity_event1.launch +++ b/launch/mcity_event1.launch @@ -17,9 +17,6 @@ - - - @@ -31,15 +28,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 diff --git a/script/controller.py b/script/controller.py index b188539..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: @@ -152,7 +63,7 @@ def __init__(self): self.odom_cb ) rospy.Subscriber( - "MSLcar0/command/trajectory", + "ego/command/trajectory", MultiDOFJointTrajectory, self.desired_waypoints_cb ) @@ -171,9 +82,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/map_updater.py b/script/map_updater.py index 7c1dce2..21ee64b 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,11 @@ 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. + + 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 ---------- @@ -52,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) @@ -168,11 +170,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 +185,10 @@ 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") + role_name = rospy.get_param("~rolename") # state information self.stateReady = False @@ -206,25 +196,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/" + role_name + "/odometry", Odometry, self.odom_cb ) rospy.Subscriber( - "MSLcar1/ground_truth/odometry", + "ado/ground_truth/odometry", Odometry, self.ado_odom_cb ) @@ -232,12 +217,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 ) @@ -272,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) @@ -326,27 +311,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() + 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 +391,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: 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 new file mode 100755 index 0000000..2a04ce6 --- /dev/null +++ b/script/trivial_planner.py @@ -0,0 +1,147 @@ +#!/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 + +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 + + 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 + + self.stateReady = False + self.state = odom_state() + + # 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 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..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()