From b04ccda3073c3d006abed261c1608c68f0a4ab66 Mon Sep 17 00:00:00 2001 From: Minghao Jiang Date: Sat, 6 Mar 2021 02:13:15 -0600 Subject: [PATCH] Hotfix on perception module (#8) * merge env_obj into raceinfo; publish left and right lane markers * lane markers msg modified * change baseline solution according to changes in perception_module; stop evalution once car reaches final waypoint Co-authored-by: Lucas --- src/graic_msgs/CMakeLists.txt | 2 +- src/graic_msgs/msg/BBList.msg | 1 - src/graic_msgs/msg/LaneInfo.msg | 7 +- src/graic_msgs/msg/LaneList.msg | 3 +- src/graic_msgs/msg/WaypointInfo.msg | 2 + .../envobj_bb_publisher/CMakeLists.txt | 19 -- .../launch/envobj_bb_publisher.launch | 16 -- .../envobj_bb_publisher/package.xml | 19 -- .../envobj_bb_publisher/setup.py | 7 - .../src/envobj_bb_publisher/__init__.py | 0 .../envobj_bb_publisher.py | 140 ----------- .../graic_raceinfo_publisher.py | 225 ++++++++++++++---- src/race/src/baseline.py | 29 ++- .../evaluation_node/src/evaluation_node.py | 15 +- .../waypoint_node/src/waypoint_node.py | 16 +- 15 files changed, 231 insertions(+), 270 deletions(-) delete mode 100644 src/graic_msgs/msg/BBList.msg create mode 100644 src/graic_msgs/msg/WaypointInfo.msg delete mode 100644 src/perception_module/envobj_bb_publisher/CMakeLists.txt delete mode 100644 src/perception_module/envobj_bb_publisher/launch/envobj_bb_publisher.launch delete mode 100644 src/perception_module/envobj_bb_publisher/package.xml delete mode 100644 src/perception_module/envobj_bb_publisher/setup.py delete mode 100644 src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/__init__.py delete mode 100644 src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/envobj_bb_publisher.py diff --git a/src/graic_msgs/CMakeLists.txt b/src/graic_msgs/CMakeLists.txt index 2329981..2d93e7e 100644 --- a/src/graic_msgs/CMakeLists.txt +++ b/src/graic_msgs/CMakeLists.txt @@ -55,9 +55,9 @@ add_message_files( ObstacleList.msg LocationInfo.msg BBInfo.msg - BBList.msg BBSingleInfo.msg EvaluationInfo.msg + WaypointInfo.msg ) ## Generate services in the 'srv' folder diff --git a/src/graic_msgs/msg/BBList.msg b/src/graic_msgs/msg/BBList.msg deleted file mode 100644 index 7e4fbb7..0000000 --- a/src/graic_msgs/msg/BBList.msg +++ /dev/null @@ -1 +0,0 @@ -BBInfo[] bounding_box_vertices \ No newline at end of file diff --git a/src/graic_msgs/msg/LaneInfo.msg b/src/graic_msgs/msg/LaneInfo.msg index 1903773..db6f3e3 100644 --- a/src/graic_msgs/msg/LaneInfo.msg +++ b/src/graic_msgs/msg/LaneInfo.msg @@ -1,8 +1,9 @@ -geometry_msgs/Vector3 location -geometry_msgs/Vector3 rotation - uint32 lane_state uint32 LEFT_LANE=3 uint32 CENTER_LANE=4 uint32 RIGHT_LANE=5 + +LaneList lane_markers_center +LaneList lane_markers_left +LaneList lane_markers_right diff --git a/src/graic_msgs/msg/LaneList.msg b/src/graic_msgs/msg/LaneList.msg index cc03a30..a5ab694 100644 --- a/src/graic_msgs/msg/LaneList.msg +++ b/src/graic_msgs/msg/LaneList.msg @@ -1 +1,2 @@ -LaneInfo[] lane_markers \ No newline at end of file +geometry_msgs/Vector3[] location +geometry_msgs/Vector3[] rotation diff --git a/src/graic_msgs/msg/WaypointInfo.msg b/src/graic_msgs/msg/WaypointInfo.msg new file mode 100644 index 0000000..1f84ab3 --- /dev/null +++ b/src/graic_msgs/msg/WaypointInfo.msg @@ -0,0 +1,2 @@ +bool isFinal +geometry_msgs/Vector3 location \ No newline at end of file diff --git a/src/perception_module/envobj_bb_publisher/CMakeLists.txt b/src/perception_module/envobj_bb_publisher/CMakeLists.txt deleted file mode 100644 index cfc2a7d..0000000 --- a/src/perception_module/envobj_bb_publisher/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(envobj_bb_publisher) - -find_package(catkin REQUIRED COMPONENTS rospy roslaunch) - -# catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - roslaunch_add_file_check(launch) -endif() - -catkin_package(CATKIN_DEPENDS rospy) - -catkin_install_python( - PROGRAMS src/envobj_bb_publisher/envobj_bb_publisher.py DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) diff --git a/src/perception_module/envobj_bb_publisher/launch/envobj_bb_publisher.launch b/src/perception_module/envobj_bb_publisher/launch/envobj_bb_publisher.launch deleted file mode 100644 index a102299..0000000 --- a/src/perception_module/envobj_bb_publisher/launch/envobj_bb_publisher.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/perception_module/envobj_bb_publisher/package.xml b/src/perception_module/envobj_bb_publisher/package.xml deleted file mode 100644 index 5bf06d5..0000000 --- a/src/perception_module/envobj_bb_publisher/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - envobj_bb_publisher - 0.0.0 - The envobj_bb_publisher package - catkin - roslaunch - rospy - nav_msgs - carla_waypoint_types - rospy - lucas - - - - TODO - - - diff --git a/src/perception_module/envobj_bb_publisher/setup.py b/src/perception_module/envobj_bb_publisher/setup.py deleted file mode 100644 index 21b99c4..0000000 --- a/src/perception_module/envobj_bb_publisher/setup.py +++ /dev/null @@ -1,7 +0,0 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['envobj_bb_publisher'], - package_dir={'': 'src'}, -) diff --git a/src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/__init__.py b/src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/envobj_bb_publisher.py b/src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/envobj_bb_publisher.py deleted file mode 100644 index 1ed3367..0000000 --- a/src/perception_module/envobj_bb_publisher/src/envobj_bb_publisher/envobj_bb_publisher.py +++ /dev/null @@ -1,140 +0,0 @@ -#!/usr/bin/env python3 -from typing import Counter -import carla -import rospy -# import sys -import numpy as np -from graic_msgs.msg import BBInfo -from graic_msgs.msg import BBList -from graic_msgs.msg import BBSingleInfo -class PerceptionModule_BB(): - def __init__(self, carla_world, role_name='ego_vehicle', radius=15): - self.sensing_radius = radius - self.world = carla_world - self.vehicle = None - self.role_name = role_name - self.find_ego_vehicle() - # find ego vehicle - # reference: - def find_ego_vehicle(self): - for actor in self.world.get_actors(): - if actor.attributes.get('role_name') == self.role_name: - self.vehicle = actor - break - - def set_radius(self, new_radius): - self.sensing_radius = new_radius - def get_radius(self): - return self.sensing_radius - # determine if the bounding box contains the given point - def boundingbox_within_range(self, box, self_loc): - radius = self.sensing_radius - box_loc = box.location - # cast vertices to the same plane as the ego vehicle - x_half_width = box.extent.x - y_half_width = box.extent.y - v1 = carla.Location(box_loc.x+x_half_width, box_loc.y+y_half_width, self_loc.z) - v2 = carla.Location(box_loc.x+x_half_width, box_loc.y-y_half_width, self_loc.z) - v3 = carla.Location(box_loc.x-x_half_width, box_loc.y+y_half_width, self_loc.z) - v4 = carla.Location(box_loc.x-x_half_width, box_loc.y-y_half_width, self_loc.z) - if self_loc.y <= v1.y and self_loc.y >= v2.y: - if (self.distance_from_point_to_line(self_loc, v1, v2) <= radius): - return True - if (self.distance_from_point_to_line(self_loc, v3, v4) <= radius): - return True - if self_loc.x <= v1.x and self_loc.x >= v3.x: - if (self.distance_from_point_to_line(self_loc, v1, v3) <= radius): - return True - if (self.distance_from_point_to_line(self_loc, v2, v4) <= radius): - return True - # otherwise calculate the distance using center of the vehicle and box - trans = carla.Transform(carla.Location(0,0,0), carla.Rotation(0,0,0)) - # obtain a vector pointing from self_loc to box center in world space - vec_from_self_to_box = box_loc - self_loc - vec_from_self_to_box.z = 0 - normalized_vec = vec_from_self_to_box/self.distance_between_points(vec_from_self_to_box, vec_from_self_to_box) - # calculate the location which is 'radius' away from the vehicle along the vec - tip_of_vec = normalized_vec*radius - loc_of_vec = tip_of_vec + self_loc - if box.contains(loc_of_vec, trans): - return True - return False - # calculate the distance from point x to the line connecting p1 and p2 - def distance_from_point_to_line(self, x, p1, p2): - num = np.abs((p2.x - p1.x)*(p1.y - x.y) - (p1.x - x.x)*(p2.y - p1.y)) - denom = np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2) - return num/denom - def distance_between_points(self, p1, p2): - return np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 + (p1.z - p2.z)**2) - # get city objects in terms of info on their bounding box - # return:vertices location in world space - # side effect: draw bounding box - def get_bb_global_ver_within_range(self, obj_type): - # TODO: need to check validity of object type - if self.vehicle == None: - self.find_ego_vehicle() - # rospy.loginfo("No ego vehicle.") - return - all_env_bbs = self.world.get_environment_objects(obj_type) - vehicle = self.vehicle - self_loc = vehicle.get_location() - radius = self.sensing_radius - filtered_obstacles = [] - for env_bb in all_env_bbs: - # center of the bounding box in world space - center_of_box = env_bb.bounding_box.location - # dist = np.sqrt((center_of_box.x - self_loc.x)**2 + (center_of_box.y-self_loc.y)**2 + (center_of_box.z-self_loc.z)**2) - dist = self.distance_between_points(center_of_box, self_loc) - if dist <= radius: - filtered_obstacles.append((env_bb.bounding_box.get_local_vertices(), env_bb.name, env_bb.id, env_bb)) - elif self.boundingbox_within_range(env_bb.bounding_box, self_loc): - filtered_obstacles.append((env_bb.bounding_box.get_local_vertices(), env_bb.name, env_bb.id, env_bb)) - return filtered_obstacles - -# publish obstacles and lane waypoints information -def publisher(percep_mod, label_list, role_name): - # main function - pub = rospy.Publisher('/carla/%s/environment_obj_bb'%role_name, BBList, queue_size=1) - rate = rospy.Rate(20) - while not rospy.is_shutdown(): - for label in label_list: - # get all vertices of all bounding boxes which are within the radius with label 'label' - vertices_of_cur_label = percep_mod.get_bb_global_ver_within_range(label) - bbs_msgs = BBList() - if not vertices_of_cur_label: - continue - for vertices_of_one_box in vertices_of_cur_label: - info = BBInfo() - for loc in vertices_of_one_box[0]: - vertex = BBSingleInfo() - vertex.vertex_location.x = loc.x - vertex.vertex_location.y = loc.y - vertex.vertex_location.z = loc.z - info.vertices_locations.append(vertex) - info.type = str(label) - info.name = str(vertices_of_one_box[1]) - info.id = vertices_of_one_box[2] % 1013 # NOTE 1013 is a magic number, the purpose is to shorten the id from a very large number - bbs_msgs.bounding_box_vertices.append(info) - # bb = vertices_of_one_box[3].bounding_box - # percep_mod.world.debug.draw_box(bb, bb.rotation, life_time=0) - pub.publish(bbs_msgs) - rate.sleep() - - -if __name__ == "__main__": - # reference: https://github.com/SIlvaMFPedro/ros_bridge/blob/master/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py - rospy.init_node('envobj_bb_publisher', anonymous=True) - # host = rospy.get_param("/carla/host", "127.0.0.1") - # port = rospy.get_param("/carla/host", 2000) - # timeout = rospy.get_param("/carla/timeout", 10) - client = carla.Client('localhost', 2000) - # client.set_timeout(timeout) - role_name = rospy.get_param("~role_name", "ego_vehicle") - # client.set_timeout(timeout) - world = client.get_world() - pm = PerceptionModule_BB(world, role_name=role_name) - default_list = [carla.CityObjectLabel.Buildings, carla.CityObjectLabel.Fences, carla.CityObjectLabel.Sidewalks, carla.CityObjectLabel.Walls, carla.CityObjectLabel.Vegetation] - try: - publisher(pm, default_list, role_name) - except rospy.exceptions.ROSInterruptException: - rospy.loginfo("Shutting down environment object publisher") \ No newline at end of file diff --git a/src/perception_module/graic_raceinfo_publisher/src/graic_raceinfo_publisher/graic_raceinfo_publisher.py b/src/perception_module/graic_raceinfo_publisher/src/graic_raceinfo_publisher/graic_raceinfo_publisher.py index 984b06d..b2232eb 100644 --- a/src/perception_module/graic_raceinfo_publisher/src/graic_raceinfo_publisher/graic_raceinfo_publisher.py +++ b/src/perception_module/graic_raceinfo_publisher/src/graic_raceinfo_publisher/graic_raceinfo_publisher.py @@ -8,17 +8,16 @@ from graic_msgs.msg import ObstacleInfo from graic_msgs.msg import ObstacleList from graic_msgs.msg import BBSingleInfo +from geometry_msgs.msg import Vector3 class PerceptionModule(): def __init__(self, carla_world, role_name, radius=15): self.sensing_radius = radius # default ????? self.world = carla_world self.vehicle = None self.role_name = role_name - self.find_ego_vehicle() # find ego vehicle - # reference: def find_ego_vehicle(self): for actor in self.world.get_actors(): if actor.attributes.get('role_name') == self.role_name: @@ -59,10 +58,67 @@ def set_radius(self, new_radius): def get_radius(self): return self.sensing_radius - + # determine if the bounding box contains the given point + def boundingbox_within_range(self, box, self_loc): + radius = self.sensing_radius + box_loc = box.location + # cast vertices to the same plane as the ego vehicle + x_half_width = box.extent.x + y_half_width = box.extent.y + v1 = carla.Location(box_loc.x+x_half_width, box_loc.y+y_half_width, self_loc.z) + v2 = carla.Location(box_loc.x+x_half_width, box_loc.y-y_half_width, self_loc.z) + v3 = carla.Location(box_loc.x-x_half_width, box_loc.y+y_half_width, self_loc.z) + v4 = carla.Location(box_loc.x-x_half_width, box_loc.y-y_half_width, self_loc.z) + if self_loc.y <= v1.y and self_loc.y >= v2.y: + if (self.distance_from_point_to_line(self_loc, v1, v2) <= radius): + return True + if (self.distance_from_point_to_line(self_loc, v3, v4) <= radius): + return True + if self_loc.x <= v1.x and self_loc.x >= v3.x: + if (self.distance_from_point_to_line(self_loc, v1, v3) <= radius): + return True + if (self.distance_from_point_to_line(self_loc, v2, v4) <= radius): + return True + # calculate distance to vertices + vertices = [v1, v2, v3, v4] + for v in vertices: + if self.distance_between_points(self_loc ,v) <= radius: + return True + return False + # calculate the distance from point x to the line connecting p1 and p2 + def distance_from_point_to_line(self, x, p1, p2): + num = np.abs((p2.x - p1.x)*(p1.y - x.y) - (p1.x - x.x)*(p2.y - p1.y)) + denom = np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2) + return num/denom + def distance_between_points(self, p1, p2): + return np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 + (p1.z - p2.z)**2) + # get city objects in terms of info on their bounding box + # return:vertices location in world space + # side effect: draw bounding box + def get_bb_global_ver_within_range(self, obj_type): + # TODO: need to check validity of object type + if self.vehicle == None: + self.find_ego_vehicle() + # rospy.loginfo("No ego vehicle.") + return + all_env_bbs = self.world.get_environment_objects(obj_type) + vehicle = self.vehicle + self_loc = vehicle.get_location() + radius = self.sensing_radius + filtered_obstacles = [] + for env_bb in all_env_bbs: + # center of the bounding box in world space + center_of_box = env_bb.bounding_box.location + # dist = np.sqrt((center_of_box.x - self_loc.x)**2 + (center_of_box.y-self_loc.y)**2 + (center_of_box.z-self_loc.z)**2) + dist = self.distance_between_points(center_of_box, self_loc) + if dist <= radius: + filtered_obstacles.append((env_bb.bounding_box.get_local_vertices(), env_bb.name, env_bb.id, env_bb.transform.location, env_bb.bounding_box)) + elif self.boundingbox_within_range(env_bb.bounding_box, self_loc): + filtered_obstacles.append((env_bb.bounding_box.get_local_vertices(), env_bb.name, env_bb.id, env_bb.transform.location, env_bb.bounding_box)) + return filtered_obstacles # get set of waypoints separated by parameter -- distance -- along the lane # reference: https://github.com/carla-simulator/carla/issues/1254 - def get_lane_way_point(self, distance=0.5): + def get_lane_markers(self, distance=0.5): if self.vehicle == None: self.find_ego_vehicle() # rospy.loginfo("No ego vehicle.") @@ -73,13 +129,34 @@ def get_lane_way_point(self, distance=0.5): # get a nearest waypoint cur_waypoint = carla_map.get_waypoint(vehicle_location) # return list of waypoints from cur_waypoint to 10 meters ahead - wp_to_end = cur_waypoint.next_until_lane_end(distance) - if len(wp_to_end) > 20: - wp_to_end = wp_to_end[0:20] - return wp_to_end -# calculate distance between p1 and p2 -def distance_between_points(p1, p2): - return np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 + (p1.z - p2.z)**2) + waypoints = cur_waypoint.next_until_lane_end(distance) + num_of_wp = len(waypoints) + if num_of_wp > 20: + waypoints = waypoints[0:20] + if num_of_wp < 20: + if waypoints[num_of_wp-1].is_junction: + # debug + waypoints.append(waypoints[num_of_wp-1].get_junction().get_waypoints(lane_type=carla.LaneType.Driving)) + # approximate a point in next lane + vec = vehicle.get_velocity() + vec = vec_end/np.sqrt((vec.x)**2 + (vec.y)**2 + (vec.z)**2) + vec = vec*2 + vec_end = waypoints[num_of_wp-1].transform.location + if num_of_wp > 1: + vec_start = waypoints[num_of_wp-2].transform.location + vec = vec_end - vec_start + vec = vec*5 + approx_next_loc = vec + vec_end + approx_waypoint = carla_map.get_waypoint(approx_next_loc) + markers_before = approx_waypoint.previous_until_lane_start(distance) + waypoints.append(markers_before) + new_num_of_markers = len(waypoints) + if new_num_of_markers < 20: + markers_after = approx_waypoint.next_until_lane_end(distance) + waypoints.append(markers_after) + if len(waypoints) > 20: + waypoints = waypoints[0:20] + return waypoints # helper function for calculating lane markers # approximate the locations of lane markers by the assumptions: # 1. the lane markers have the same z coordinates as the current location @@ -94,78 +171,135 @@ def get_markers(cur_loc, v, w): n2 = (-x/y)*m2 marker1 = cur_loc + carla.Location(m1, n1, 0) marker2 = cur_loc + carla.Location(m2, n2, 0) - return (marker1, marker2) + x1 = np.array([marker1.x, marker1.y, marker1.z]) + x2 = np.array([x, y, z]) + if np.cross(x1, x2)[2] > 0: + return (marker1, marker2) + else: + return (marker2, marker1) +# helper function for creating lane markers information +def get_marker_info(loc, rot): + mark_loc = Vector3() + mark_rot = Vector3() + mark_loc.x = loc.x + mark_loc.y = loc.y + mark_loc.z = loc.z + mark_rot.x = rot.pitch + mark_rot.y = rot.yaw + mark_rot.z = rot.roll + return (mark_loc, mark_rot) # publish obstacles and lane waypoints information -def publisher(percep_mod, role_name): +def publisher(percep_mod, role_name, label_list): # main function obs_pub = rospy.Publisher('/carla/%s/obstacles'%role_name, ObstacleList, queue_size=1) - lane_pub = rospy.Publisher('/carla/%s/lane_markers'%role_name, LaneList, queue_size=1) + lane_pub = rospy.Publisher('/carla/%s/lane_markers'%role_name, LaneInfo, queue_size=1) rate = rospy.Rate(20) draw_counter = 0 while not rospy.is_shutdown(): obs = percep_mod.get_all_obstacles_within_range() - lp = percep_mod.get_lane_way_point() - obsmsg = [] - lpmsg = [] + lp = percep_mod.get_lane_markers() + obs_msg = [] + # mark_msg_center = [] + # mark_msg_left = [] + # mark_msg_right = [] draw_counter += 1 if obs is None or lp is None: continue for ob in obs: - temp = ObstacleInfo() + info = ObstacleInfo() # fill the fields for current obstacle - temp.obstacle_name = ob.type_id - temp.obstacle_id = ob.id + info.obstacle_name = ob.type_id + info.obstacle_id = ob.id loc = ob.get_location() - temp.location.x = loc.x - temp.location.y = loc.y - temp.location.z = loc.z + info.location.x = loc.x + info.location.y = loc.y + info.location.z = loc.z # draw bounding box for vehicles and walkers if 'vehicle' in ob.type_id or 'pedestrian' in ob.type_id: bb = ob.bounding_box bb.location = loc bb.rotation = ob.get_transform().rotation if draw_counter % 8 == 0: - percep_mod.world.debug.draw_box(bb, bb.rotation, color=carla.Color(255, 0, 0), life_time=0.5) + percep_mod.world.debug.draw_box(bb, bb.rotation, color=carla.Color(255, 0, 0), life_time=1) vertices = bb.get_local_vertices() for v in vertices: vertex = BBSingleInfo() vertex.vertex_location.x = v.x vertex.vertex_location.y = v.y vertex.vertex_location.z = v.z - temp.vertices_locations.append(vertex) - obsmsg.append(temp) + info.vertices_locations.append(vertex) + obs_msg.append(info) + # TODO: fix left and right markers' rotation + # TODO: add gradient + marker_msg = LaneInfo() + marker_center_list = LaneList() + marker_left_list = LaneList() + marker_right_list = LaneList() for i in range(len(lp)): p = lp[i] - temp = LaneInfo() trans = p.transform loc = trans.location rot = trans.rotation - temp.location.x = loc.x - temp.location.y = loc.y - temp.location.z = loc.z - temp.rotation.x = rot.pitch - temp.rotation.y = rot.yaw - temp.rotation.z = rot.roll - temp.lane_state = abs(p.lane_id) - lpmsg.append(temp) + mark_loc, mark_rot = get_marker_info(loc, rot) + marker_msg.lane_state = abs(p.lane_id) + marker_center_list.location.append(mark_loc) + marker_center_list.rotation.append(mark_rot) # draw left and right lane markers width = p.lane_width next_p = None if i != len(lp) - 1: next_p = lp[i+1] vec = next_p.transform.location - loc + mark1 = carla.Location(loc.x-width/2, loc.y,loc.z) + mark2 = carla.Location(loc.x+width/2, loc.y,loc.z) if vec.y == 0: - mark1 = carla.Location(loc.x+width/2, loc.y,loc.z) - mark2 = carla.Location(loc.x-width/2, loc.y,loc.z) - percep_mod.world.debug.draw_point(mark1,life_time=2) - percep_mod.world.debug.draw_point(mark2,life_time=2) + mark_loc, mark_rot = get_marker_info(mark1, rot) + marker_left_list.location.append(mark_loc) + marker_left_list.rotation.append(mark_rot) + mark_loc, mark_rot = get_marker_info(mark2, rot) + marker_right_list.location.append(mark_loc) + marker_right_list.rotation.append(mark_rot) + percep_mod.world.debug.draw_point(mark1,life_time=1) + percep_mod.world.debug.draw_point(mark2,life_time=1) else: mark1, mark2 = get_markers(loc, vec, width) - percep_mod.world.debug.draw_point(mark1,life_time=2) - percep_mod.world.debug.draw_point(mark2,life_time=2) - percep_mod.world.debug.draw_point(trans.location,life_time=2) - obs_pub.publish(obsmsg) - lane_pub.publish(lpmsg) + mark_loc, mark_rot = get_marker_info(mark1, rot) + marker_left_list.location.append(mark_loc) + marker_left_list.rotation.append(mark_rot) + mark_loc, mark_rot = get_marker_info(mark2, rot) + marker_right_list.location.append(mark_loc) + marker_right_list.rotation.append(mark_rot) + percep_mod.world.debug.draw_point(mark1,life_time=1) + percep_mod.world.debug.draw_point(mark2,life_time=1) + percep_mod.world.debug.draw_point(trans.location,life_time=1) + marker_msg.lane_markers_center = marker_center_list + marker_msg.lane_markers_left = marker_left_list + marker_msg.lane_markers_right = marker_right_list + # NOTE Environment objects note used now + # for label in label_list: + # # get all vertices of all bounding boxes which are within the radius with label 'label' + # vertices_of_cur_label = percep_mod.get_bb_global_ver_within_range(label) + # if not vertices_of_cur_label: + # continue + # for vertices_of_one_box in vertices_of_cur_label: + # info = ObstacleInfo() + # for loc in vertices_of_one_box[0]: + # vertex = BBSingleInfo() + # vertex.vertex_location.x = loc.x + # vertex.vertex_location.y = loc.y + # vertex.vertex_location.z = loc.z + # info.vertices_locations.append(vertex) + # info.obstacle_name = str(vertices_of_one_box[1]) + # info.obstacle_id = vertices_of_one_box[2] % 1013 # NOTE 1013 is a magic number, the purpose is to shorten the id from a very large number + # obstacle_loc = vertices_of_one_box[3] + # info.location.x = obstacle_loc.x + # info.location.y = obstacle_loc.y + # info.location.z = obstacle_loc.z + # bb = vertices_of_one_box[4] + # percep_mod.world.debug.draw_box(bb, bb.rotation, life_time=1) + # obs_msg.append(info) + obs_pub.publish(obs_msg) + lane_pub.publish(marker_msg) rate.sleep() @@ -180,7 +314,8 @@ def publisher(percep_mod, role_name): # client.set_timeout(timeout) world = client.get_world() pm = PerceptionModule(world, role_name) + default_list = [carla.CityObjectLabel.Buildings, carla.CityObjectLabel.Fences, carla.CityObjectLabel.Sidewalks, carla.CityObjectLabel.Walls, carla.CityObjectLabel.Vegetation] try: - publisher(pm, role_name) + publisher(pm, role_name, default_list) except rospy.exceptions.ROSInterruptException: rospy.loginfo("Shutting down raceinfo publisher") diff --git a/src/race/src/baseline.py b/src/race/src/baseline.py index f97ab37..34b82be 100644 --- a/src/race/src/baseline.py +++ b/src/race/src/baseline.py @@ -3,7 +3,7 @@ import argparse import time from graic_msgs.msg import ObstacleList, ObstacleInfo -from graic_msgs.msg import LocationInfo +from graic_msgs.msg import LocationInfo, WaypointInfo from ackermann_msgs.msg import AckermannDrive from carla_msgs.msg import CarlaEgoVehicleControl from graic_msgs.msg import LaneList @@ -11,13 +11,14 @@ class VehicleDecision(): def __init__(self, role_name): - self.subWaypoint = rospy.Subscriber("/carla/%s/lane_markers"%role_name, LaneList, self.waypointCallback) + self.subLaneMarker = rospy.Subscriber("/carla/%s/lane_markers"%role_name, LaneInfo, self.lanemarkerCallback) + self.subWaypoint = rospy.Subscriber("/carla/%s/waypoints"%role_name, WaypointInfo, self.waypointCallback) self.vehicle_state = 'straight' self.lane_state = 0 self.counter = 0 - self.waypoint = None + self.lane_marker = None self.target_x = None self.target_y = None self.change_lane = False @@ -25,13 +26,18 @@ def __init__(self, role_name): self.detect_dist = 15 self.speed = 20 + self.reachEnd = False - def waypointCallback(self, data): - self.waypoint = data.lane_markers[-1] - self.lane_state = self.waypoint.lane_state + + def lanemarkerCallback(self, data): + self.lane_marker = data.lane_markers_center.location[-1] + self.lane_state = data.lane_state if not self.target_x or not self.target_y: - self.target_x = self.waypoint.location.x - self.target_y = self.waypoint.location.y + self.target_x = self.lane_marker.x + self.target_y = self.lane_marker.y + + def waypointCallback(self, data): + self.reachEnd = data.isFinal def get_ref_state(self, currState, obstacleList): @@ -42,6 +48,9 @@ def get_ref_state(self, currState, obstacleList): obstacleList: List of obstacles Outputs: reference state position and velocity of the vehicle """ + if self.reachEnd: + return None + # print("Reach end: ", self.reachEnd) curr_x = currState[0][0] curr_y = currState[0][1] @@ -122,8 +131,8 @@ def get_ref_state(self, currState, obstacleList): prev_target_x = self.target_x prev_target_y = self.target_y - self.target_x = self.waypoint.location.x - self.target_y = self.waypoint.location.y + self.target_x = self.lane_marker.x + self.target_y = self.lane_marker.y target_orientation = np.arctan2(self.target_y-prev_target_y, self.target_x-prev_target_x) diff --git a/src/race_util_module/evaluation_node/src/evaluation_node.py b/src/race_util_module/evaluation_node/src/evaluation_node.py index e30c1aa..cb659ad 100644 --- a/src/race_util_module/evaluation_node/src/evaluation_node.py +++ b/src/race_util_module/evaluation_node/src/evaluation_node.py @@ -2,7 +2,7 @@ import numpy as np from carla_msgs.msg import CarlaCollisionEvent from carla_msgs.msg import CarlaLaneInvasionEvent -from graic_msgs.msg import LocationInfo, EvaluationInfo +from graic_msgs.msg import LocationInfo, EvaluationInfo, WaypointInfo from geometry_msgs.msg import Vector3 from std_msgs.msg import Int16, Float32, String import time @@ -21,7 +21,7 @@ class EvaluationNode: def __init__(self, world, role_name='ego_vehicle', track='t1_triple'): self.subCollision = rospy.Subscriber('/carla/%s/collision'%role_name, CarlaCollisionEvent, self.collisionCallback) self.subLocation = rospy.Subscriber('/carla/%s/location'%role_name, LocationInfo, self.locationCallback) - self.subWaypoint = rospy.Subscriber('/carla/%s/waypoints'%role_name, Vector3, self.waypointCallback) + self.subWaypoint = rospy.Subscriber('/carla/%s/waypoints'%role_name, WaypointInfo, self.waypointCallback) self.subLaneInvasion = rospy.Subscriber('carla/%s/lane_invasion'%role_name, CarlaLaneInvasionEvent, self.laneCallback) self.pubReach = rospy.Publisher('/carla/%s/reached'%role_name, String, queue_size=1) self.pubScore = rospy.Publisher('/carla/%s/score'%role_name, Float32, queue_size=1) @@ -34,6 +34,7 @@ def __init__(self, world, role_name='ego_vehicle', track='t1_triple'): self.location = None self.role_name = role_name self.waypoint = None + self.reachEnd = False self.obs_map = {} self.world = world if track == 't1_triple': @@ -57,6 +58,10 @@ def locationCallback(self, data): def collisionCallback(self, data): if not self.obs_map: return + + if self.reachEnd: + return + hitObj = self.obs_map[str(data.other_actor_id)]+"_at_time_"+str(datetime.timedelta( seconds=int(rospy.get_rostime().to_sec()))) if hitObj in self.hitObjects: @@ -70,9 +75,13 @@ def collisionCallback(self, data): self.score -= collisionPenalty def waypointCallback(self, data): - self.waypoint = data + self.waypoint = data.location + self.reachEnd = data.isFinal def laneCallback(self, data): + if self.reachEnd: + return + for marking in data.crossed_lane_markings: if marking is CarlaLaneInvasionEvent.LANE_MARKING_SOLID: self.score -= deviationPenalty diff --git a/src/race_util_module/waypoint_node/src/waypoint_node.py b/src/race_util_module/waypoint_node/src/waypoint_node.py index e7367a0..2901712 100644 --- a/src/race_util_module/waypoint_node/src/waypoint_node.py +++ b/src/race_util_module/waypoint_node/src/waypoint_node.py @@ -2,6 +2,7 @@ import numpy as np from geometry_msgs.msg import Vector3 from std_msgs.msg import String +from graic_msgs.msg import WaypointInfo import time import pickle import carla @@ -29,6 +30,9 @@ def getWaypoint(self): else: return None + def isFinal(self): + return len(self.waypoint_list) == 1 + # TODO change this for new map def reachCallback(self, data): if len(self.waypoint_list) > 0: @@ -37,14 +41,16 @@ def reachCallback(self, data): def run(wn, role_name): rate = rospy.Rate(20) # 20 Hz - pubWaypoint = rospy.Publisher('/carla/%s/waypoints'%role_name, Vector3, queue_size=1) + pubWaypoint = rospy.Publisher('/carla/%s/waypoints'%role_name, WaypointInfo, queue_size=1) while not rospy.is_shutdown(): waypoint = wn.getWaypoint() + pub_waypoint = WaypointInfo() if not waypoint: - continue - pub_waypoint = Vector3() - pub_waypoint.x = waypoint[0] - pub_waypoint.y = waypoint[1] + pub_waypoint.isFinal = True + else: + pub_waypoint.location.x = waypoint[0] + pub_waypoint.location.y = waypoint[1] + pub_waypoint.location.z = waypoint[2] pubWaypoint.publish(pub_waypoint) rate.sleep()