def getActors(self): """Call simulator and get instances of actors and store them, create list of derived_object_msgs """ # self.ros_actors.clear() del self.carla_actors[:] del self.ros_actors[:] for actor in self.world.get_actors(): if actor.attributes.get('role_name') in ['ego_vehicle', 'hero']: self.ego_vehicle = actor self.scenario_xml.ego_vehicle = actor elif actor.type_id.startswith("vehicle") or actor.type_id.startswith("walker"): self.carla_actors.append(actor) derived_obj = Object(detection_level=self.getActorProb(actor.id)) derived_obj.id = actor.id derived_obj.shape.type = SolidPrimitive.BOX derived_obj.shape.dimensions = [ actor.bounding_box.extent.x*2, actor.bounding_box.extent.y*2, actor.bounding_box.extent.z*2 ] # if actor.type_id in bike_blueprints: # derived_obj.classification = Object.CLASSIFICATION_BIKE # elif actor.type_id in motorcycle_blueprints: # derived_obj.classification = Object.CLASSIFICATION_MOTORCYCLE # elif actor.type_id in truck_blueprints: # derived_obj.classification = Object.CLASSIFICATION_TRUCK # elif actor.type_id.startswith("vehicle"): if actor.type_id.startswith("vehicle"): derived_obj.classification = Object.CLASSIFICATION_CAR elif actor.type_id.startswith("walker"): derived_obj.classification = Object.CLASSIFICATION_PEDESTRIAN else: derived_obj.classification = Object.CLASSIFICATION_OTHER_VEHICLE self.ros_actors.append(derived_obj) elif actor.type_id.startswith('static'): self.carla_actors.append(actor) derived_obj = Object(detection_level=self.getActorProb(actor.id)) derived_obj.id = actor.id derived_obj.shape.type = SolidPrimitive.BOX # print(self.scenario_xml.blueprint.find(actor.type_id).size) size = 0.5 # size = self.scenario_xml.blueprint.find(actor.type_id).size derived_obj.shape.dimensions = [ float(size)*2, float(size)*2, float(size)*2 ] derived_obj.classification = Object.CLASSIFICATION_UNKNOWN self.ros_actors.append(derived_obj)
def get_object_info(self): """ Function to send object messages of this traffic participant. A derived_object_msgs.msg.Object is prepared to be published via '/carla/objects' :return: """ obj = Object(header=self.get_msg_header("map")) # ID obj.id = self.get_id() # Pose obj.pose = self.get_current_ros_pose() # Twist obj.twist = self.get_current_ros_twist() # Acceleration obj.accel = self.get_current_ros_accel() # Shape obj.shape.type = SolidPrimitive.BOX obj.shape.dimensions.extend([ self.carla_actor.bounding_box.extent.x * 2.0, self.carla_actor.bounding_box.extent.y * 2.0, self.carla_actor.bounding_box.extent.z * 2.0]) # Classification if available in attributes if self.get_classification() != Object.CLASSIFICATION_UNKNOWN: obj.object_classified = True obj.classification = self.get_classification() obj.classification_certainty = 1.0 obj.classification_age = self.classification_age return obj
def send_object_msg(self): """ Function to send object messages of this vehicle. A derived_object_msgs.msg.Object is prepared to be published via '/carla/objects' :return: """ vehicle_object = Object(header=self.get_msg_header()) # ID vehicle_object.id = self.get_global_ID() # Pose vehicle_object.pose = self.get_current_ros_pose() # Twist vehicle_object.twist = self.get_current_ros_twist() # Acceleration vehicle_object.accel = self.get_current_ros_accel() # Shape vehicle_object.shape.type = SolidPrimitive.BOX vehicle_object.shape.dimensions.extend([ self.carla_actor.bounding_box.extent.x * 2.0, self.carla_actor.bounding_box.extent.y * 2.0, self.carla_actor.bounding_box.extent.z * 2.0 ]) # Classification if available in attributes if self.classification != Object.CLASSIFICATION_UNKNOWN: vehicle_object.object_classified = True vehicle_object.classification = self.classification vehicle_object.classification_certainty = 1.0 self.classification_age += 1 vehicle_object.classification_age = self.classification_age # self.publish_ros_message('/carla/objects', vehicle_object) return vehicle_object
def create_object(obj_id=0, classification=Object.CLASSIFICATION_UNKNOWN, x_pos=0, y_pos=0, z_pos=0): """Create Object with predefined parameters""" object_msg = Object() object_msg.id = obj_id object_msg.pose.position = Point(x_pos, y_pos, z_pos) object_msg.classification = classification object_msg.shape.dimensions = [2, 2, 2] return object_msg
def lidar_callback(msg): t1 = time.time() msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg) frame_id = msg.header.frame_id np_p = get_xyz_points(msg_cloud, True) print(msg.header.stamp.to_sec()) print(rospy.Time.now().to_sec()) print(np_p.size) scores, dt_box_lidar, types = proc_1.run(np_p) bbox_corners3d = boxes_to_corners_3d(dt_box_lidar) empty_markers = MarkerArray() clear_marker = Marker() clear_marker.header.stamp = rospy.Time.now() clear_marker.header.frame_id = frame_id clear_marker.ns = "objects" clear_marker.id = 0 clear_marker.action = clear_marker.DELETEALL clear_marker.lifetime = rospy.Duration() empty_markers.markers.append(clear_marker) pub_bbox_array.publish(empty_markers) bbox_arry = MarkerArray() if scores.size != 0: for i in range(scores.size): point_list = [] bbox = Marker() bbox.type = Marker.LINE_LIST bbox.ns = "objects" bbox.id = i box = bbox_corners3d[i] q = yaw2quaternion(float(dt_box_lidar[i][6])) for j in range(24): p = Point() point_list.append(p) point_list[0].x = float(box[0, 0]) point_list[0].y = float(box[0, 1]) point_list[0].z = float(box[0, 2]) point_list[1].x = float(box[1, 0]) point_list[1].y = float(box[1, 1]) point_list[1].z = float(box[1, 2]) point_list[2].x = float(box[1, 0]) point_list[2].y = float(box[1, 1]) point_list[2].z = float(box[1, 2]) point_list[3].x = float(box[2, 0]) point_list[3].y = float(box[2, 1]) point_list[3].z = float(box[2, 2]) point_list[4].x = float(box[2, 0]) point_list[4].y = float(box[2, 1]) point_list[4].z = float(box[2, 2]) point_list[5].x = float(box[3, 0]) point_list[5].y = float(box[3, 1]) point_list[5].z = float(box[3, 2]) point_list[6].x = float(box[3, 0]) point_list[6].y = float(box[3, 1]) point_list[6].z = float(box[3, 2]) point_list[7].x = float(box[0, 0]) point_list[7].y = float(box[0, 1]) point_list[7].z = float(box[0, 2]) point_list[8].x = float(box[4, 0]) point_list[8].y = float(box[4, 1]) point_list[8].z = float(box[4, 2]) point_list[9].x = float(box[5, 0]) point_list[9].y = float(box[5, 1]) point_list[9].z = float(box[5, 2]) point_list[10].x = float(box[5, 0]) point_list[10].y = float(box[5, 1]) point_list[10].z = float(box[5, 2]) point_list[11].x = float(box[6, 0]) point_list[11].y = float(box[6, 1]) point_list[11].z = float(box[6, 2]) point_list[12].x = float(box[6, 0]) point_list[12].y = float(box[6, 1]) point_list[12].z = float(box[6, 2]) point_list[13].x = float(box[7, 0]) point_list[13].y = float(box[7, 1]) point_list[13].z = float(box[7, 2]) point_list[14].x = float(box[7, 0]) point_list[14].y = float(box[7, 1]) point_list[14].z = float(box[7, 2]) point_list[15].x = float(box[4, 0]) point_list[15].y = float(box[4, 1]) point_list[15].z = float(box[4, 2]) point_list[16].x = float(box[0, 0]) point_list[16].y = float(box[0, 1]) point_list[16].z = float(box[0, 2]) point_list[17].x = float(box[4, 0]) point_list[17].y = float(box[4, 1]) point_list[17].z = float(box[4, 2]) point_list[18].x = float(box[1, 0]) point_list[18].y = float(box[1, 1]) point_list[18].z = float(box[1, 2]) point_list[19].x = float(box[5, 0]) point_list[19].y = float(box[5, 1]) point_list[19].z = float(box[5, 2]) point_list[20].x = float(box[2, 0]) point_list[20].y = float(box[2, 1]) point_list[20].z = float(box[2, 2]) point_list[21].x = float(box[6, 0]) point_list[21].y = float(box[6, 1]) point_list[21].z = float(box[6, 2]) point_list[22].x = float(box[3, 0]) point_list[22].y = float(box[3, 1]) point_list[22].z = float(box[3, 2]) point_list[23].x = float(box[7, 0]) point_list[23].y = float(box[7, 1]) point_list[23].z = float(box[7, 2]) for j in range(24): bbox.points.append(point_list[j]) bbox.scale.x = 0.1 bbox.color.a = 1.0 bbox.color.r = 1.0 bbox.color.g = 0.0 bbox.color.b = 0.0 #bbox.header.stamp = rospy.Time.now() bbox.header.stamp = msg.header.stamp bbox.header.frame_id = frame_id bbox_arry.markers.append(bbox) # add text text_show = Marker() text_show.type = Marker.TEXT_VIEW_FACING text_show.ns = "objects" text_show.header.stamp = msg.header.stamp text_show.header.frame_id = frame_id text_show.id = i + scores.size text_show.pose = Pose(position=Point( float(dt_box_lidar[i][0]), float(dt_box_lidar[i][1]), float(dt_box_lidar[i][2]) + 2.0)) text_show.text = str( proc_1.net.class_names[types[i] - 1]) + ' ' + str( round(scores[i], 2)) text_show.action = Marker.ADD text_show.color.a = 1.0 text_show.color.r = 1.0 text_show.color.g = 1.0 text_show.color.b = 0.0 text_show.scale.z = 1.5 bbox_arry.markers.append(text_show) print("total callback time: ", time.time() - t1) pub_bbox_array.publish(bbox_arry) cloud_array = xyz_array_to_pointcloud2(np_p[:, :3], rospy.Time.now(), frame_id) pub_point2_.publish(cloud_array) ## publish to fusion msg_lidar_objects = ObjectArray() msg_lidar_objects.header.stamp = msg.header.stamp msg_lidar_objects.header.frame_id = frame_id # get points in each box t3 = time.time() num_obj = dt_box_lidar.shape[0] point_indices = roiaware_pool3d_utils.points_in_boxes_cpu( torch.from_numpy(np_p[:, 0:3]), torch.from_numpy(dt_box_lidar)).numpy() # (nboxes, npoints) t4 = time.time() print(f"get points in each box cost time: {t4 - t3}") print('') #clustered_points = [] #for test if scores.size != 0: for i in range(scores.size): lidar_object = Object() lidar_object.header.stamp = msg.header.stamp lidar_object.header.frame_id = frame_id lidar_object.id = i lidar_object.pose.position.x = float(dt_box_lidar[i][0]) lidar_object.pose.position.y = float(dt_box_lidar[i][1]) lidar_object.pose.position.z = float(dt_box_lidar[i][2]) lidar_object.pose.orientation = yaw2quaternion( float(dt_box_lidar[i][6])) lidar_object.shape.dimensions.append(float(dt_box_lidar[i][3])) lidar_object.shape.dimensions.append(float(dt_box_lidar[i][4])) lidar_object.shape.dimensions.append(float(dt_box_lidar[i][5])) lidar_object.classification = types[i] gt_points = np_p[point_indices[i] > 0] for pp in range(gt_points.shape[0]): temp_point = Point32() temp_point.x = gt_points[pp][0] temp_point.y = gt_points[pp][1] temp_point.z = gt_points[pp][2] lidar_object.polygon.points.append(temp_point) #clustered_points.append(gt_points[pp,:].tolist()) msg_lidar_objects.objects.append(lidar_object) # clustered_points = np.array(clustered_points) # cloud_array = xyz_array_to_pointcloud2(clustered_points[:, :3], rospy.Time.now(), frame_id) # pub_point2_.publish(cloud_array) pub_object_array.publish(msg_lidar_objects)