def callback(msg):
if not candidate_pose:
return
target_array = BoundingBoxArray()
target_array.header.stamp = msg.header.stamp
target_array.header.frame_id = "world"
target = BoundingBox()
target.header.stamp = msg.header.stamp
target.header.frame_id = "world"
target.pose = candidate_pose
target.dimensions.x = 0.2
target.dimensions.y = 0.2
target.dimensions.z = 0.2
target_array.boxes = [target]
pub_target.publish(target_array)
candidate_array = BoundingBoxArray()
candidate_array.header.stamp = msg.header.stamp
candidate_array.header.frame_id = "world"
for x in [-0.2, -0.1, 0.0, 0.1, 0.2]:
for y in [-0.2, -0.1, 0.0, 0.1, 0.2]:
for z in [-0.2, -0.1, 0.0, 0.1, 0.2]:
candidate = BoundingBox()
candidate.header.stamp = msg.header.stamp
candidate.header.frame_id = "world"
candidate.pose.position.z = 2 + z
candidate.pose.position.x = x
candidate.pose.position.y = y
candidate.pose.orientation.w = 1.0
candidate.dimensions.x = 0.1
candidate.dimensions.y = 0.1
candidate.dimensions.z = 0.1
candidate_array.boxes.append(candidate)
pub_candidate.publish(candidate_array)
def callback_with_cluster_box(self, cluster_boxes_msg, instance_boxes_msg, instance_label_msg):
labeled_cluster_boxes = BoundingBoxArray()
labeled_instance_boxes = BoundingBoxArray()
labeled_cluster_boxes.header = cluster_boxes_msg.header
labeled_instance_boxes.header = instance_boxes_msg.header
for index, box in enumerate(cluster_boxes_msg.boxes):
if not box.pose.position.x == 0.0:
tmp_box = BoundingBox()
tmp_box.header = box.header
tmp_box.pose = box.pose
tmp_box.dimensions = box.dimensions
# TODO fix index indent, jsk_pcl_ros_utils/label_to_cluster_point_indices_nodelet.cpp
tmp_box.label = index + 1
labeled_cluster_boxes.boxes.append(tmp_box)
for box, label in zip(instance_boxes_msg.boxes, instance_label_msg.labels):
tmp_box = BoundingBox()
tmp_box.header = box.header
tmp_box.pose = box.pose
tmp_box.dimensions = box.dimensions
tmp_box.label = label.id
labeled_instance_boxes.boxes.append(tmp_box)
self.labeled_cluster_boxes_pub.publish(labeled_cluster_boxes)
self.labeled_instance_boxes_pub.publish(labeled_instance_boxes)
def pose_msg_callback(self, pose, classes):
bboxes = BoundingBoxArray(header=pose.header)
for p in pose.poses:
b = BoundingBox(header=pose.header)
b.pose = p
bboxes.boxes.append(b)
self.box_msg_callback(bboxes, classes)
def to_msg(self, boxes, class_ids, header):
box_arr = BoundingBoxArray()
box_arr.header = header
for i in range(len(boxes)):
if str(self.classes[class_ids[i]]) == "person":
x, y, w, h = boxes[i]
box = BoundingBox()
box.label = i
box.value = 0
box.pose.position.x = x
box.pose.position.y = y
box.pose.position.z = 0
box.pose.orientation.x = 0
box.pose.orientation.y = 0
box.pose.orientation.x = 0
box.pose.orientation.w = 0
box.dimensions.x = w
box.dimensions.y = h
box.dimensions.z = 0
box_arr.boxes.append(box)
return box_arr
def get_nearest_box(self, req):
distance = 100
has_request_item = False
nearest_box = BoundingBox()
for index, box in enumerate(self.boxes.boxes):
if box.pose.position.x == 0 or \
box.pose.position.y == 0 or \
box.pose.position.z == 0:
rospy.logwarn('boxes has (0, 0, 0) position box')
continue
if self.label_lst[box.label] == req.label:
has_request_item = True
ref_point = np.array([box.pose.position.x + (box.dimensions.x * 0.5),
box.pose.position.y + (box.dimensions.y * 0.5),
box.pose.position.z + (box.dimensions.z * 0.5)])
target_point = np.array([req.target.x,
req.target.y,
req.target.z])
if np.linalg.norm(ref_point - target_point) < distance:
nearest_box.pose = box.pose
nearest_box.dimensions = box.dimensions
distance = np.linalg.norm(ref_point - target_point)
return nearest_box, has_request_item
def init_boundingboxarray(self, num_boxes=30):
self.boundingBoxArray_object = BoundingBoxArray()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now(
) # Note you need to call rospy.init_node() before this will work
h.frame_id = "world"
self.boundingBoxArray_object.header = h
self.minimum_dimension = 0.2
self.init_x_position = 1.0
for i in range(num_boxes):
new_box = BoundingBox()
new_box.header = h
new_box.pose = Pose()
new_box.pose.position.x = self.init_x_position + i * self.minimum_dimension
new_box.dimensions = Vector3()
new_box.dimensions.x = self.minimum_dimension
new_box.dimensions.y = self.minimum_dimension
new_box.dimensions.z = self.minimum_dimension
new_box.label = i
new_box.value = i * self.minimum_dimension
self.boundingBoxArray_object.boxes.append(new_box)
self.publish_once(self.boundingBoxArray_object)
def create_bb_msg(self, cur_bb, is_map_coord=False):
bb_corners = np.array([cur_bb['upper_left_px'], cur_bb['lower_right_px']])
if is_map_coord is True:
bb_corners_map_coord = bb_corners
else:
bb_corners_px_coord = bb_corners
bb_corners_map_coord = self.img2map.pixel2map(bb_corners_px_coord)
bb_corners_mean = np.mean(bb_corners_map_coord, axis=0)
bb_corners_diff = np.abs(bb_corners_map_coord[0, :] - bb_corners_map_coord[1, :])
cur_bb_msg = BoundingBox()
cur_bb_msg.header.frame_id = 'map'
cur_bb_msg.header.stamp = rospy.Time.now()
cur_bb_msg.pose.position.x = bb_corners_mean[0]
cur_bb_msg.pose.position.y = bb_corners_mean[1]
cur_bb_msg.pose.position.z = 0
cur_bb_msg.pose.orientation.x = 0
cur_bb_msg.pose.orientation.y = 0
cur_bb_msg.pose.orientation.z = 0
cur_bb_msg.pose.orientation.w = 1
cur_bb_msg.dimensions.x = bb_corners_diff[0]
cur_bb_msg.dimensions.y = bb_corners_diff[1]
cur_bb_msg.dimensions.z = 0.1
cur_bb_msg.value = 1.0
cur_bb_msg.label = 1
return cur_bb_msg
def rslidar_callback(msg):
t_t = time.time()
#calib = getCalibfromFile(calib_file)
#calib = getCalibfromROS(calibmsg)
frame = msg.header.seq
arr_bbox = BoundingBoxArray()
msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
np_p = get_xyz_points(msg_cloud, True)
print(" ")
#scores, dt_box_lidar, types = proc_1.run(np_p)
scores, dt_box_lidar, types, pred_dict = proc_1.run(np_p, calib, frame)
annos_sorted = sortbydistance(dt_box_lidar, scores, types)
#pp_AB3DMOT_list = anno_to_AB3DMOT(pred_dict, msg)
pp_AB3DMOT_list = anno_to_AB3DMOT(dt_box_lidar, scores, types, msg)
pp_list = anno_to_sort(dt_box_lidar, scores, types)
pp_3D_list = anno_to_3Dsort(annos_sorted, types)
MarkerArray_list = anno_to_rviz(dt_box_lidar, scores, types, msg)
if scores.size != 0:
for i in range(scores.size):
if scores[i] > threshold:
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
bbox.header.stamp = rospy.Time.now()
q = yaw2quaternion(float(dt_box_lidar[i][6]))
bbox.pose.orientation.x = q[1]
bbox.pose.orientation.y = q[2]
bbox.pose.orientation.z = q[3]
bbox.pose.orientation.w = q[0]
bbox.pose.position.x = float(
dt_box_lidar[i][0]) - movelidarcenter
bbox.pose.position.y = float(dt_box_lidar[i][1])
bbox.pose.position.z = float(dt_box_lidar[i][2])
bbox.dimensions.x = float(dt_box_lidar[i][3])
bbox.dimensions.y = float(dt_box_lidar[i][4])
bbox.dimensions.z = float(dt_box_lidar[i][5])
bbox.value = scores[i]
bbox.label = int(types[i])
arr_bbox.boxes.append(bbox)
print("total callback time: ", time.time() - t_t)
arr_bbox.header.frame_id = msg.header.frame_id
arr_bbox.header.stamp = msg.header.stamp
if len(arr_bbox.boxes) is not 0:
pub_arr_bbox.publish(arr_bbox)
arr_bbox.boxes = []
else:
arr_bbox.boxes = []
pub_arr_bbox.publish(arr_bbox)
pubRviz.publish(MarkerArray_list)
pubSort.publish(pp_list)
pub3DSort.publish(pp_3D_list)
pubAB3DMOT.publish(pp_AB3DMOT_list)
def viz_bbox(position, q, size, i):
bbox = BoundingBox()
bbox.pose.position = position
bbox.pose.orientation = Quaternion(*q)
bbox.dimensions = size
bbox.label = i
return bbox
def rslidar_callback(msg):
t_t = time.time()
arr_bbox = BoundingBoxArray()
#t = time.time()
msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
np_p = get_xyz_points(msg_cloud, True)
print(" ")
#print("prepare cloud time: ", time.time() - t)
#t = time.time()
scores, dt_box_lidar, types = proc_1.run(np_p)
#print("network forward time: ", time.time() - t)
# filter_points_sum = []
#t = time.time()
if scores.size != 0:
for i in range(scores.size):
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
bbox.header.stamp = rospy.Time.now()
q = yaw2quaternion(float(dt_box_lidar[i][6]))
bbox.pose.orientation.x = q[0]
bbox.pose.orientation.y = q[1]
bbox.pose.orientation.z = q[2]
bbox.pose.orientation.w = q[3]
bbox.pose.position.x = float(dt_box_lidar[i][0])
bbox.pose.position.y = float(dt_box_lidar[i][1])
bbox.pose.position.z = float(dt_box_lidar[i][2])
bbox.dimensions.x = float(dt_box_lidar[i][3])
bbox.dimensions.y = float(dt_box_lidar[i][4])
bbox.dimensions.z = float(dt_box_lidar[i][5])
bbox.value = scores[i]
bbox.label = types[i]
arr_bbox.boxes.append(bbox)
# filter_points = np_p[point_indices[:,i]]
# filter_points_sum.append(filter_points)
#print("publish time cost: ", time.time() - t)
# filter_points_sum = np.concatenate(filter_points_sum, axis=0)
# filter_points_sum = filter_points_sum[:, :3]
# print("output of concatenate:", filter_points_sum)
# filter_points_sum = np.arange(24).reshape(8,3)
# cluster_cloud = xyz_array_to_pointcloud2(filter_points_sum, stamp=rospy.Time.now(), frame_id=msg.header.frame_id)
# pub_segments.publish(cluster_cloud)
print("total callback time: ", time.time() - t_t)
arr_bbox.header.frame_id = msg.header.frame_id
arr_bbox.header.stamp = rospy.Time.now()
if len(arr_bbox.boxes) is not 0:
pub_arr_bbox.publish(arr_bbox)
arr_bbox.boxes = []
else:
arr_bbox.boxes = []
pub_arr_bbox.publish(arr_bbox)
def __init__(self):
self.srv = Server(DynamicParamsConfig,
self.dynamic_reconfigure_callback)
self.sub = rospy.Subscriber(
"~input", PolygonArray, self.polygon_callback)
self.box = BoundingBox()
self.frame = rospy.get_param("~frame", "/base_link")
self.box.header.frame_id = self.frame
self.pub = rospy.Publisher("~output", BoundingBox, queue_size=1)
def callback(self, box_msg):
labeled_boxes = {}
label_buf = []
orientation = Pose().orientation
for box in box_msg.boxes:
if box.label in label_buf:
labeled_boxes[box.label] += [self.get_points(box)]
orientation = box.pose.orientation
else:
labeled_boxes[box.label] = [self.get_points(box)]
label_buf.append(box.label)
bounding_box_msg = BoundingBoxArray()
for label, boxes in zip(labeled_boxes.keys(), labeled_boxes.values()):
thresh = self.thresh
if self.label_lst[label] == 'shelf_flont':
thresh = 2.0
clustering = Clustering()
boxes = np.array(boxes)
result = clustering.clustering_wrapper(boxes, thresh)
for cluster in result:
max_candidates = [
boxes[i][0] + (boxes[i][1] * 0.5) for i in cluster.indices
]
min_candidates = [
boxes[i][0] - (boxes[i][1] * 0.5) for i in cluster.indices
]
candidates = np.array(max_candidates + min_candidates)
dimension = candidates.max(axis=0) - candidates.min(axis=0)
center = candidates.min(axis=0) + (dimension * 0.5)
distances = self.get_distances(
np.array([boxes[i][0] for i in cluster.indices]), [
np.linalg.norm(boxes[i][1]) * 0.5
for i in cluster.indices
])
tmp_box = BoundingBox()
tmp_box.header = box_msg.header
tmp_box.dimensions.x = dimension[0]
tmp_box.dimensions.y = dimension[1]
tmp_box.dimensions.z = dimension[2]
tmp_box.pose.position.x = center[0]
tmp_box.pose.position.y = center[1]
tmp_box.pose.position.z = center[2]
tmp_box.pose.orientation = orientation
tmp_box.label = label
tmp_box.value = distances.mean()
bounding_box_msg.boxes.append(tmp_box)
bounding_box_msg.header = box_msg.header
self.box_pub.publish(bounding_box_msg)
def callback(msg):
box_array = BoundingBoxArray()
box_array.header = msg.header
for footstep in msg.footsteps:
box = BoundingBox()
box.header = msg.header
box.pose = footstep.pose
box.dimensions = footstep.dimensions
box_array.boxes.append(box)
pub.publish(box_array)
def convert(header, xx_cube):
boundingboxarray_msg = BoundingBoxArray()
boundingboxarray_msg.header = header
num_xx = int(xx_cube.shape[0] / 8)
for i in range(num_xx):
cube_raw = xx_cube[range(8 * i, 8 * i + 8), :]
bb_in_camera = np.column_stack((cube_raw, np.ones((8, 1))))
bb_in_lidar = np.linalg.inv(calib.lidar_to_cam).dot(bb_in_camera.T).T
boundingbox_msg = BoundingBox()
boundingbox_msg.header = boundingboxarray_msg.header
# boundingbox中心点位置
boundingbox_msg.pose.position.x = bb_in_lidar[:, 0].mean()
boundingbox_msg.pose.position.y = bb_in_lidar[:, 1].mean()
boundingbox_msg.pose.position.z = bb_in_lidar[:, 2].mean()
# 寻找y坐标最小的顶点，计算相邻两个顶点的旋转角及边长
bb_bottom = bb_in_lidar[:4]
min_idx = np.where(bb_bottom[:, 1] == bb_bottom[:, 1].min())[0][0]
theta = math.atan2(
bb_bottom[(min_idx + 1) % 4, 1] - bb_bottom[min_idx, 1],
bb_bottom[(min_idx + 1) % 4, 0] - bb_bottom[min_idx, 0])
b_1 = (
(bb_bottom[(min_idx + 1) % 4, 1] - bb_bottom[min_idx, 1])**2 +
(bb_bottom[(min_idx + 1) % 4, 0] - bb_bottom[min_idx, 0])**2)**0.5
b_2 = (
(bb_bottom[(min_idx + 3) % 4, 1] - bb_bottom[min_idx, 1])**2 +
(bb_bottom[(min_idx + 3) % 4, 0] - bb_bottom[min_idx, 0])**2)**0.5
if theta < 90 * math.pi / 180:
rotation_angle = theta
dimension_x = b_1
dimension_y = b_2
else:
rotation_angle = theta - 90 * math.pi / 180
dimension_x = b_2
dimension_y = b_1
# boundingbox旋转角四元数
boundingbox_msg.pose.orientation.x = 0
boundingbox_msg.pose.orientation.y = 0
boundingbox_msg.pose.orientation.z = math.sin(0.5 * rotation_angle)
boundingbox_msg.pose.orientation.w = math.cos(0.5 * rotation_angle)
# boundingbox尺寸
boundingbox_msg.dimensions.x = dimension_x
boundingbox_msg.dimensions.y = dimension_y
boundingbox_msg.dimensions.z = bb_in_lidar[:, 2].max(
) - bb_in_lidar[:, 2].min()
boundingbox_msg.value = 0
boundingbox_msg.label = 0
boundingboxarray_msg.boxes.append(boundingbox_msg)
return boundingboxarray_msg
def lidar_callback(self, msg):
if self.model.inference_ctx is None or self.model.inference_ctx.anchor_cache is None:
return
for field in msg.fields:
if field.name == "i" or field.name == "intensity":
intensity_fname = field.name
intensity_dtype = field.datatype
else:
intensity_fname = None
intensity_dtype = None
dtype_list = self._fields_to_dtype(msg.fields, msg.point_step)
pc_arr = np.frombuffer(msg.data, dtype_list)
if intensity_fname:
pc_arr = structured_to_unstructured(
pc_arr[["x", "y", "z", intensity_fname]]).copy()
if intensity_dtype == 2:
pc_arr[:, 3] = pc_arr[:, 3] / 255
else:
pc_arr = structured_to_unstructured(pc_arr[["x", "y", "z"]]).copy()
pc_arr = np.hstack((pc_arr, np.zeros((pc_arr.shape[0], 1))))
lidar_boxes = self.model.predcit(pc_arr)
num_detects = len(lidar_boxes)
arr_bbox = BoundingBoxArray()
for i in range(num_detects):
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
bbox.header.stamp = rospy.Time.now()
bbox.pose.position.x = float(lidar_boxes[i][0])
bbox.pose.position.y = float(lidar_boxes[i][1])
bbox.pose.position.z = float(
lidar_boxes[i][2]) + float(lidar_boxes[i][5]) / 2
bbox.dimensions.x = float(lidar_boxes[i][3]) # width
bbox.dimensions.y = float(lidar_boxes[i][4]) # length
bbox.dimensions.z = float(lidar_boxes[i][5]) # height
q = Quaternion(axis=(0, 0, 1), radians=float(lidar_boxes[i][6]))
bbox.pose.orientation.x = q.x
bbox.pose.orientation.y = q.y
bbox.pose.orientation.z = q.z
bbox.pose.orientation.w = q.w
arr_bbox.boxes.append(bbox)
arr_bbox.header.frame_id = msg.header.frame_id
arr_bbox.header.stamp = rospy.Time.now()
print("Number of detections: {}".format(num_detects))
self.pub_bbox.publish(arr_bbox)
def detector_callback(self, pcl_msg):
start = time.time()
# rospy.loginfo('Processing Pointcloud with PointRCNN')
arr_bbox = BoundingBoxArray()
seq = pcl_msg.header.seq
stamp = pcl_msg.header.stamp
# in message pointcloud has x pointing forward, y pointing to the left and z pointing upward
pts_lidar = np.array([[
p[0], p[1], p[2], p[3]
] for p in pc2.read_points(
pcl_msg, skip_nans=True, field_names=("x", "y", "z", "intensity"))
],
dtype=np.float32)
scores, dt_box_lidar, types = self.run_model(pts_lidar)
# TODO: question convert into torch tensors? torch.from_numpy(pts_lidar)
# move onto gpu if available
# TODO: check if needs translation/rotation to compensate for tilt etc.
if scores.size != 0:
for i in range(scores.size):
bbox = BoundingBox()
bbox.header.frame_id = pcl_msg.header.frame_id
bbox.header.stamp = rospy.Time.now()
# bbox.header.seq = pcl_msg.header.seq
q = yaw2quaternion(float(dt_box_lidar[i][6]))
bbox.pose.orientation.x = q[1]
bbox.pose.orientation.y = q[2]
bbox.pose.orientation.z = q[3]
bbox.pose.orientation.w = q[0]
bbox.pose.position.x = float(dt_box_lidar[i][0])
bbox.pose.position.y = float(dt_box_lidar[i][1])
bbox.pose.position.z = float(dt_box_lidar[i][2])
bbox.dimensions.x = float(dt_box_lidar[i][3])
bbox.dimensions.y = float(dt_box_lidar[i][4])
bbox.dimensions.z = float(dt_box_lidar[i][5])
bbox.value = scores[i]
bbox.label = int(types[i])
arr_bbox.boxes.append(bbox)
# rospy.loginfo("3D detector time: {}".format(time.time() - start))
arr_bbox.header.frame_id = pcl_msg.header.frame_id
arr_bbox.header.stamp = pcl_msg.header.stamp
arr_bbox.header.seq = pcl_msg.header.seq
if len(arr_bbox.boxes) != 0:
self.pub_arr_bbox.publish(arr_bbox)
arr_bbox.boxes = []
else:
arr_bbox.boxes = []
self.pub_arr_bbox.publish(arr_bbox)
def callback(msg):
box_array = BoundingBoxArray()
box_array.header = msg.header
for footstep in msg.footsteps:
box = BoundingBox()
box.header = msg.header
box.pose = footstep.pose
box.dimensions = footstep.dimensions
box.pose.position.z += (z_max + z_min) / 2.0
box.dimensions.z = z_max - z_min
box_array.boxes.append(box)
pub.publish(box_array)
def labeled_pose_callback(self, pose_msg):
self.header = pose_msg.header
self.qatm_boxes = BoundingBoxArray()
self.qatm_boxes.header = pose_msg.header
for pose in pose_msg.poses:
tmp_box = BoundingBox()
tmp_box.header = pose_msg.header
tmp_box.pose = pose.pose
tmp_box.dimensions.x = 0.03
tmp_box.dimensions.y = 0.03
tmp_box.dimensions.z = 0.03
tmp_box.label = self.label_lst.index(pose.label)
self.qatm_boxes.boxes.append(tmp_box)
def people_msg_callback(self, people, classes):
bboxes = BoundingBoxArray(header=people.header)
for p in people.poses:
b = BoundingBox()
for i, n in enumerate(p.limb_names):
if n in ["Neck", "Nose", "REye", "LEye", "REar", "LEar"]:
b.header = people.header
b.pose = p.poses[i]
break
if not b.header.frame_id:
b.header = people.header
b.pose = b.poses[0]
self.box_msg_callback(bboxes, classes)
def callback(self, instance_boxes_msg, instance_label_msg):
labeled_instance_boxes = BoundingBoxArray()
labeled_instance_boxes.header = instance_boxes_msg.header
for box, label in zip(instance_boxes_msg.boxes, instance_label_msg.labels):
tmp_box = BoundingBox()
tmp_box.header = box.header
tmp_box.pose = box.pose
tmp_box.dimensions = box.dimensions
tmp_box.label = label.id
labeled_instance_boxes.boxes.append(tmp_box)
self.labeled_instance_boxes_pub.publish(labeled_instance_boxes)
def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
d = {}
pictograms = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text)
rz = float(p.find('rz').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
b = BoundingBox()
b.pose.position = Vector3(tx, ty, tz/2.0)
b.pose.orientation = Quaternion(*q)
b.dimensions = size
b.label = i
picto_text = Pictogram()
picto_text.mode = Pictogram.STRING_MODE
picto_text.pose.position = Vector3(tx, ty, -tz/2.0)
q = tf.transformations.quaternion_from_euler(0.7, 0.0, -0.7)
picto_text.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5)
picto_text.size = 1
picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1)
picto_text.character = label
if d.has_key(frame + j) == True:
d[frame + j].append(b)
pictograms[frame + j].append(picto_text)
else:
d[frame + j] = [b]
pictograms[frame + j] = [picto_text]
return d, pictograms
def get_box(msg):
box = BoundingBox()
get_box_pose_srv = rospy.ServiceProxy(
"/transformable_server_sample/get_pose", GetTransformableMarkerPose)
resp = get_box_pose_srv(target_name=msg.data)
box.pose = resp.pose_stamped.pose
box.header = resp.pose_stamped.header
get_box_dim_srv = rospy.ServiceProxy(
"/transformable_server_sample/get_dimensions", GetMarkerDimensions)
resp2 = get_box_dim_srv(target_name=msg.data)
box.dimensions.x = resp2.dimensions.x
box.dimensions.y = resp2.dimensions.y
box.dimensions.z = resp2.dimensions.z
return box
def listen_transform(self, parent_frame, child_frame):
box = BoundingBox()
try:
self.listener.waitForTransform(
parent_frame, child_frame, rospy.Time(0), rospy.Duration(3.0))
(trans, rot) = self.listener.lookupTransform(
parent_frame, child_frame, rospy.Time(0))
box.pose.position = Point(trans[0], trans[1], trans[2])
box.pose.orientation = Quaternion(rot[0], rot[1], rot[2], rot[3])
return box
except:
rospy.logwarn('cannot lookup transform')
return box
def __init__(self):
self.srv = Server(DynamicParamsConfig,
self.dynamic_reconfigure_callback)
self.pub = rospy.Publisher("~output", BoundingBox, queue_size=1)
self.box = BoundingBox()
self.frame = rospy.get_param("~frame", "/base_link")
self.box.header.frame_id = self.frame
self.x = rospy.get_param("~x", 0.6)
self.y = rospy.get_param("~y", 0.0)
self.z = rospy.get_param("~z", 0.2)
self.w = rospy.get_param("~w", 0.6)
self.d = rospy.get_param("~d", 0.6)
self.h = rospy.get_param("~h", 0.2)
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
bboxs = {}
pictograms = {}
arrows = {}
paths = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
track_id = int(v.find('trackid').text)
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text) + 0.85
rz = float(p.find('rz').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
bbox = BoundingBox()
picto = Pictogram()
arrow = Marker()
bbox = viz_bbox(Vector3(tx, ty, tz), q, size, track_id)
picto = viz_picto(Vector3(tx, ty, -tz / 2.0), q, label)
arrow = viz_arrow(Vector3(tx, ty, tz), q, track_id)
if bboxs.has_key(frame + j) == True:
bboxs[frame + j].append(bbox)
pictograms[frame + j].append(picto)
arrows[frame + j].append(arrow)
else:
bboxs[frame + j] = [bbox]
pictograms[frame + j] = [picto]
arrows[frame + j] = [arrow]
return bboxs, pictograms, arrows
def velo_callback(msg):
global sess, model
arr_bbox = BoundingBoxArray()
pcl_msg = pc2.read_points(msg,
skip_nans=False,
field_names=("x", "y", "z", "intensity", "ring"))
np_p = np.array(list(pcl_msg), dtype=np.float32)
# np_p = np.delete(np_p, -1, 1) # delete "ring" field
print(np_p.shape)
dataset = dataset_generator(np_p, batch_size=1, multi_gpu_sum=1)
# print("{} {} {} {}".format(dataset[0],dataset[1],dataset[2],dataset[3])) # DEBUG
results = model.predict_step_ros(sess, dataset)
# publish to /velodyne_poitns_modified
publish_test(np_p, msg.header.frame_id)
# results: (N, N') (class, x, y, z, h, w, l, rz, score)
if len(results[0]) != 0:
# print("len(results[0]) : {} ".format(len(results[0])))
for result in results[0]:
# print("[+] result: {}".format(result)) # DEBUG
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
# bbox.header.stamp = rospy.Time.now()
# print("result[7] : {} ".format(result[7]))
q = quaternion_from_euler(0, 0, float(result[7]))
bbox.pose.orientation.x = q[0]
bbox.pose.orientation.y = q[1]
bbox.pose.orientation.z = q[2]
bbox.pose.orientation.w = q[3]
bbox.pose.position.x = float(result[1])
bbox.pose.position.y = float(result[2])
bbox.pose.position.z = float(result[3])
bbox.dimensions.x = float(result[6])
bbox.dimensions.y = float(result[5])
bbox.dimensions.z = float(result[4])
arr_bbox.boxes.append(bbox)
arr_bbox.header.frame_id = msg.header.frame_id
# arr_bbox.header.stamp = rospy.Time.now()
# print("arr_bbox.boxes.size() : {} ".format(len(arr_bbox.boxes)))
if len(arr_bbox.boxes) is not 0:
pub_arr_bbox.publish(arr_bbox)
arr_bbox.boxes.clear()
def cb(msg):
out_msg = BoundingBox()
out_msg.header.stamp = msg.header.stamp
out_msg.header.frame_id = rospy.get_param('~frame_id', 'map')
out_msg.pose.position.x = 0
out_msg.pose.position.y = 0
out_msg.pose.position.z = 0
out_msg.pose.orientation.x = 0
out_msg.pose.orientation.y = 0
out_msg.pose.orientation.z = 0
out_msg.pose.orientation.w = 1
out_msg.dimensions.x = 0.3
out_msg.dimensions.y = 0.3
out_msg.dimensions.z = 0.3
pub.publish(out_msg)
def dummyBoundingBoxPublisher():
pub = rospy.Publisher('/dummy_bounding_box', BoundingBox, queue_size=1)
rospy.init_node('dummyBoundingBoxPublisher_node', anonymous=True)
rate = rospy.Rate(25)
boundingBox_object = BoundingBox()
i = 0
pose_object = Pose()
dimensions_object = Vector3()
minimum_dimension = 0.2
boundingBox_object.label = 1234
while not rospy.is_shutdown():
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now(
) # Note you need to call rospy.init_node() before this will work
h.frame_id = "world"
boundingBox_object.header = h
sinus_value = math.sin(i / 10.0)
boundingBox_object.value = sinus_value
# Change Pose to see effects
pose_object.position.x = 1.0
pose_object.position.y = 0.0
pose_object.position.z = sinus_value
# ai, aj, ak == roll, pitch, yaw
quaternion = tf.transformations.quaternion_from_euler(ai=0,
aj=0,
ak=sinus_value)
pose_object.orientation.x = quaternion[0]
pose_object.orientation.y = quaternion[1]
pose_object.orientation.z = quaternion[2]
pose_object.orientation.w = quaternion[3]
dimensions_object.x = sinus_value / 10 + minimum_dimension
dimensions_object.y = minimum_dimension
dimensions_object.z = minimum_dimension
# Assign pose and dimension objects
boundingBox_object.pose = pose_object
boundingBox_object.dimensions = dimensions_object
pub.publish(boundingBox_object)
rate.sleep()
i += 1
def _pub_bboxes_callback(self, event):
bbox_array_msg = BoundingBoxArray()
bbox_array_msg.header.frame_id = self.config['boxes'][0]['frame_id']
bbox_array_msg.header.stamp = event.current_real
for box in self.config['boxes']:
bbox_msg = BoundingBox()
pose = self.object_poses[box['name']]
bbox_msg.header.frame_id = bbox_array_msg.header.frame_id
bbox_msg.header.stamp = bbox_array_msg.header.stamp
bbox_msg.pose = pose
dimensions = self.object_dimensions[box['name']]
bbox_msg.dimensions.x = dimensions.x
bbox_msg.dimensions.y = dimensions.y
bbox_msg.dimensions.z = dimensions.z
bbox_array_msg.boxes.append(bbox_msg)
self.pub_bboxes.publish(bbox_array_msg)
def _cb(self, event):
self.box = BoundingBox()
self.box.header.stamp = event.current_real
self.box.header.frame_id = 'dummy_frame'
self.box.pose.position.x = 1.0
self.box.pose.position.y = 2.0
self.box.pose.position.z = 3.0
self.box.pose.orientation.x = 4.0
self.box.pose.orientation.y = 5.0
self.box.pose.orientation.z = 6.0
self.box.pose.orientation.w = 7.0
self.box.dimensions.x = 8.0
self.box.dimensions.y = 9.0
self.box.dimensions.z = 10.0
self.box.value = 11.0
self.box.label = 12
self.pub.publish(self.box)
