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)
