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 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 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(box_array, coef_array):
# We assume that the size of box_array and polygon_array are 1
if len(box_array.boxes) > 0 and len(coef_array.coefficients) > 0:
if (box_array.boxes[0].header.frame_id ==
coef_array.coefficients[0].header.frame_id):
box = box_array.boxes[0]
box_pos = numpy.array([
box.pose.position.x, box.pose.position.y, box.pose.position.z
])
coef = coef_array.coefficients[0].values
n = numpy.array([coef[0], coef[1], coef[2]])
d = (coef[0] * box_pos[0] + coef[1] * box_pos[1] +
coef[2] * box_pos[2] +
coef[3] / math.sqrt(coef[0] * coef[0] + coef[1] * coef[1] +
coef[2] * coef[2]))
required_distance = distance - d
rospy.loginfo("required_distance: %f" % (required_distance))
new_box_pos = required_distance * n + box_pos
box.pose.position.x = new_box_pos[0]
box.pose.position.y = new_box_pos[1]
box.pose.position.z = new_box_pos[2]
result_box_array = BoundingBoxArray()
result_box_array.header = box_array.header
result_box_array.boxes = [box]
pub_box_array.publish(result_box_array)
else:
rospy.logwarn("frame_id of box array and coef array are not same")
else:
rospy.logwarn("Size of box array and coef array are not enough")
def on_people(self, ppl_msg):
"""
ppl_msg (jsk_recognition_msgs/PeoplePoseArray)
refer: https://github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/output.md
"""
frame_id = remove_slash(ppl_msg.header.frame_id)
in_objects = self.objects
objects = []
for bbox in in_objects:
if remove_slash(bbox.header.frame_id) != frame_id:
ps = PoseStamped(header=bbox.header, pose=bbox.pose)
try:
ps = self.tfl.transform(ps, frame_id)
bbox.header, bbox.pose = ps.header, ps.pose
objects.append(bbox)
except tf2_ros.TransformException:
continue
out_bboxes = []
out_markers = []
for i, pose in enumerate(ppl_msg.poses):
# for each person
if "rarm" in self.use_arm:
rwrist = find_pose("RWrist", pose)
rfinger = find_pose(["RHand5", "RHand6", "RHand7", "RHand8"],
pose)
if rwrist is not None and rfinger is not None:
rclosest, rdist = self.get_closest_bbox((rwrist, rfinger),
objects)
if rdist is not None and\
self.min_dist_threshold <= rdist <= self.max_dist_threshold:
out_bboxes.append(rclosest)
rmarker = self.get_marker((rwrist, rfinger), frame_id)
rmarker.id = 2 * i
out_markers.append(rmarker)
if "larm" in self.use_arm:
lwrist = find_pose("LWrist", pose)
lfinger = find_pose(["LHand5", "LHand6", "LHand7", "LHand8"],
pose)
if lwrist is not None and lfinger is not None:
lclosest, ldist = self.get_closest_bbox((lwrist, lfinger),
objects)
if ldist is not None and\
self.min_dist_threshold <= ldist <= self.max_dist_threshold:
out_bboxes.append(lclosest)
lmarker = self.get_marker((lwrist, lfinger), frame_id)
lmarker.id = 2 * i + 1
out_markers.append(lmarker)
# publish
if out_bboxes:
msg = BoundingBoxArray()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.boxes = out_bboxes
self.pub_bbox.publish(msg)
if out_markers:
self.pub_marker.publish(MarkerArray(markers=out_markers))
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(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(msg):
global pub, frame_id, min_z, max_z, tf_listener
boxes = msg.boxes
# latest
(pos, rot) = tf_listener.lookupTransform(msg.header.frame_id, frame_id, rospy.Time(0))
origin_pose = concatenate_matrices(
translation_matrix(pos), quaternion_matrix(rot))
result = BoundingBoxArray()
result.header = msg.header
min_z_pos = 1000000.0
min_box = None
for box in msg.boxes:
box_pos = [box.pose.position.x, box.pose.position.y, box.pose.position.z]
# [x, y, z, w]
box_rot = [box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z,
box.pose.orientation.w]
box_pose = concatenate_matrices(
translation_matrix(box_pos), quaternion_matrix(box_rot))
box_global_pose = concatenate_matrices(origin_pose, box_pose)
box_global_pos = translation_from_matrix(box_global_pose)
if box_global_pos[2] > min_z and box_global_pos[2] < max_z:
result.boxes.append(box)
if box_global_pos[2] < min_z_pos:
min_z_pos = box_global_pos[2]
min_box = box
if len(result.boxes) > 1:
result.boxes = [min_box]
pub.publish(result)
def callback(msg):
global pub, frame_id, min_z, max_z, tf_listener
boxes = msg.boxes
# latest
(pos, rot) = tf_listener.lookupTransform(msg.header.frame_id, frame_id,
rospy.Time(0))
origin_pose = concatenate_matrices(translation_matrix(pos),
quaternion_matrix(rot))
result = BoundingBoxArray()
result.header = msg.header
min_z_pos = 1000000.0
min_box = None
for box in msg.boxes:
box_pos = [
box.pose.position.x, box.pose.position.y, box.pose.position.z
]
# [x, y, z, w]
box_rot = [
box.pose.orientation.x, box.pose.orientation.y,
box.pose.orientation.z, box.pose.orientation.w
]
box_pose = concatenate_matrices(translation_matrix(box_pos),
quaternion_matrix(box_rot))
box_global_pose = concatenate_matrices(origin_pose, box_pose)
box_global_pos = translation_from_matrix(box_global_pose)
if box_global_pos[2] > min_z and box_global_pos[2] < max_z:
result.boxes.append(box)
if box_global_pos[2] < min_z_pos:
min_z_pos = box_global_pos[2]
min_box = box
if len(result.boxes) > 1:
result.boxes = [min_box]
pub.publish(result)
def rslidar_callback(msg):
# t_t = time.time()
arr_bbox = BoundingBoxArray()
msg_cloud = ros_numpy.point_cloud2.pointcloud2_to_array(msg)
np_p = get_xyz_points(msg_cloud, True)
print(" ")
seq = msg.header.seq
stamp = msg.header.stamp
input_points = {
'stamp': stamp,
'seq': seq,
'points': np_p
}
if(proc_1.get_lidar_data(input_points)):
scores, dt_box_lidar, types = proc_1.run()
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][8]))
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][4])
bbox.dimensions.y = float(dt_box_lidar[i][3])
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)
def on_people(self, ppl_msg):
"""
ppl_msg (jsk_recognition_msgs/PeoplePoseArray)
refer: https://github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/output.md
"""
frame_id = remove_slash(ppl_msg.header.frame_id)
in_objects = self.objects
objects = []
for bbox in in_objects:
if remove_slash(bbox.header.frame_id) != frame_id:
ps = PoseStamped(header=bbox.header, pose=bbox.pose)
try:
ps = self.tfl.transform(ps, frame_id)
bbox.header, bbox.pose = ps.header, ps.pose
objects.append(bbox)
except tf2_ros.TransformException:
continue
out_bboxes = []
out_markers = []
for i, pose in enumerate(ppl_msg.poses):
# for each person
rwrist = find_pose("RWrist", pose)
rfinger = find_pose(["RHand5", "RHand6", "RHand7", "RHand8"], pose)
if rwrist is not None and rfinger is not None:
rclosest, rdist = self.get_closest_bbox((rwrist, rfinger), objects)
if rdist is not None and rdist < self.dist_threshold:
out_bboxes.append(rclosest)
rmarker = self.get_marker((rwrist, rfinger), frame_id)
rmarker.id = 2 * i
out_markers.append(rmarker)
lwrist = find_pose("LWrist", pose)
lfinger = find_pose(["LHand5", "LHand6", "LHand7", "LHand8"], pose)
if lwrist is not None and lfinger is not None:
lclosest, ldist = self.get_closest_bbox((lwrist, lfinger), objects)
if ldist is not None and ldist < self.dist_threshold:
out_bboxes.append(lclosest)
lmarker = self.get_marker((lwrist, lfinger), frame_id)
lmarker.id = 2 * i + 1
out_markers.append(lmarker)
# publish
if out_bboxes:
msg = BoundingBoxArray()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.boxes = out_bboxes
self.pub_bbox.publish(msg)
if out_markers:
self.pub_marker.publish(MarkerArray(markers=out_markers))
def create_bb_array_msg(self, bb_msg_list):
"""Create a BoundingBoxArray msg from a bounding box list dict (output from
self.create_bb_msg_list).
Args:
bounding_box_list: A list of bounding box dicts as defined in read_bounding_boxes().
Returns:
oh_bb: An instance of the BoundingBoxArray msg.
"""
oh_bb = BoundingBoxArray()
oh_bb.header.frame_id = 'map'
oh_bb.header.stamp = rospy.Time.now()
oh_bb.boxes = bb_msg_list
return oh_bb
def LeadCarUpdate(msg):
global lidar_array, filter_boxes
leadc_boxes = BoundingBoxArray()
leadc_boxes.header = filter_boxes.header
compare_boxes = BoundingBoxArray()
compare_boxes.header = filter_boxes.header
compare_boxes.boxes = filter_boxes.boxes[:]
if len(msg.data)==0:
print('no lead car')
else:
rc_x = msg.data[0].pos_x
rc_y = msg.data[0].pos_y
rc_v = msg.data[0].speed
distances = []
for i in range(len(compare_boxes.boxes)):
lidar_x = compare_boxes.boxes[i].pose.position.x
lidar_y = compare_boxes.boxes[i].pose.position.y
distance = np.sqrt((rc_x-lidar_x)**2+(rc_y-lidar_y)**2)
distances.append(distance)
print(min(distances))
if (min(distances)<5):
obj_i = distances.index(min(distances))
leadc_boxes.boxes.append(compare_boxes.boxes[obj_i])
else:
leadc_box = BoundingBox()
leadc_box.header = filter_boxes.boxes[1].header
leadc_box.pose.position.x = rc_x
leadc_box.pose.position.y = rc_y
leadc_box.pose.position.z = .5
leadc_box.pose.orientation.x = 0
leadc_box.pose.orientation.y = 0
leadc_box.pose.orientation.z = 0
leadc_box.pose.orientation.w = 0
leadc_box.dimensions.x = 5
leadc_box.dimensions.y = 5
leadc_box.dimensions.z = 5
leadc_boxes.boxes.append(leadc_box)
pub_leadc.publish(leadc_boxes)
def callback(boxes_msg):
# we expect odom
(trans, rot) = tf_listener.lookupTransform(frame_id,
boxes_msg.header.frame_id,
rospy.Time(0))
origin_matrix = concatenate_matrices(translation_matrix(trans),
quaternion_matrix(rot))
min_y_pose = None
min_y_dist = 100.0
min_y_box = None
for box in boxes_msg.boxes:
pose = box.pose
# pose to matrix
pose_matrix = concatenate_matrices(
translation_matrix(
(pose.position.x, pose.position.y, pose.position.z)),
quaternion_matrix(((pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w))))
box_pose = concatenate_matrices(origin_matrix, pose_matrix)
box_pos = translation_from_matrix(box_pose)
if (box.dimensions.x < min_x or box.dimensions.x > max_x
or box.dimensions.y < min_y or box.dimensions.y > max_y
or box.dimensions.z < min_z or box.dimensions.z > max_z):
continue
if 0.8 < box_pos[2] and box_pos[2] < 1.0:
if min_y_dist > abs(box_pos[1]):
min_y_dist = abs(box_pos[1])
min_y_pose = pose
min_y_box = box
if min_y_pose:
pose_msg = PoseStamped()
pose_msg.header = boxes_msg.header
pose_msg.pose = min_y_pose
pub.publish(pose_msg)
box_arr = BoundingBoxArray()
box_arr.header = boxes_msg.header
box_arr.boxes = [min_y_box]
pub_box.publish(box_arr)
rospy.loginfo("Success to detect handle bbx")
return
rospy.logerr("Failed to detect handle bbox")
def talker():
pub = rospy.Publisher('/segmentation_decomposer/boxes',
BoundingBoxArray,
queue_size=10)
rospy.init_node('segmentation_talker', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
b = BoundingBoxArray()
b.header.frame_id = "/head_mount_kinect_rgb_optical_frame"
b.header.stamp = rospy.get_rostime()
b1 = BoundingBox()
# b1.header.frame_id = "/head_mount_kinect_rgb_optical_frame"
# b1.header.stamp = rospy.get_rostime()
b1.header = b.header
b1.pose.position.x = 0.1
b1.pose.position.y = 0.1
b1.pose.position.z = 0.1
b1.dimensions.x = 0.15
b1.dimensions.y = 0.06
b1.dimensions.z = 0.06
b.boxes = [b1]
pub.publish(b)
rate.sleep()
def callback(box_array, coef_array):
# We assume that the size of box_array and polygon_array are 1
if len(box_array.boxes) > 0 and len(coef_array.coefficients) > 0:
if (box_array.boxes[0].header.frame_id == coef_array.coefficients[0].header.frame_id):
box = box_array.boxes[0]
box_pos = numpy.array([box.pose.position.x, box.pose.position.y, box.pose.position.z])
coef = coef_array.coefficients[0].values
n = numpy.array([coef[0], coef[1], coef[2]])
d = (coef[0] * box_pos[0] + coef[1] * box_pos[1] + coef[2] * box_pos[2] + coef[3] /
math.sqrt(coef[0] * coef[0] + coef[1] * coef[1] + coef[2] * coef[2]))
required_distance = distance - d
rospy.loginfo("required_distance: %f" % (required_distance))
new_box_pos = required_distance * n + box_pos
box.pose.position.x = new_box_pos[0]
box.pose.position.y = new_box_pos[1]
box.pose.position.z = new_box_pos[2]
result_box_array = BoundingBoxArray()
result_box_array.header = box_array.header
result_box_array.boxes = [box]
pub_box_array.publish(result_box_array)
else:
rospy.logwarn("frame_id of box array and coef array are not same")
else:
rospy.logwarn("Size of box array and coef array are not enough")
def callback(boxes_msg):
# we expect odom
(trans, rot) = tf_listener.lookupTransform(frame_id, boxes_msg.header.frame_id, rospy.Time(0))
origin_matrix = concatenate_matrices(translation_matrix(trans),
quaternion_matrix(rot))
min_y_pose = None
min_y_dist = 100.0
min_y_box = None
for box in boxes_msg.boxes:
pose = box.pose
# pose to matrix
pose_matrix = concatenate_matrices(translation_matrix((pose.position.x, pose.position.y, pose.position.z)),
quaternion_matrix(((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))))
box_pose = concatenate_matrices(origin_matrix, pose_matrix)
box_pos = translation_from_matrix(box_pose)
if (box.dimensions.x < min_x or box.dimensions.x > max_x
or box.dimensions.y < min_y or box.dimensions.y > max_y
or box.dimensions.z < min_z or box.dimensions.z > max_z):
continue
if 0.8 < box_pos[2] and box_pos[2] < 1.0:
if min_y_dist > abs(box_pos[1]):
min_y_dist = abs(box_pos[1])
min_y_pose = pose
min_y_box = box
if min_y_pose:
pose_msg = PoseStamped()
pose_msg.header = boxes_msg.header
pose_msg.pose = min_y_pose
pub.publish(pose_msg)
box_arr = BoundingBoxArray()
box_arr.header = boxes_msg.header
box_arr.boxes = [min_y_box]
pub_box.publish(box_arr)
rospy.loginfo("Success to detect handle bbx")
return
rospy.logerr("Failed to detect handle bbox")
r = rospy.Rate(10)
theta = 0
while not rospy.is_shutdown():
boxes = BoundingBoxArray()
theta = math.fmod(theta + 0.1, math.pi * 2)
box = BoundingBox()
box.header.stamp = rospy.Time.now()
box.header.frame_id = "camera_link"
box.pose.orientation.w = 1
box.pose.position.x = 0.8 * math.cos(theta)
box.pose.position.y = 0.8 * math.sin(theta)
box.pose.position.z = 0.1
box.dimensions.x = 0.1
box.dimensions.y = 0.1
box.dimensions.z = 0.1
box2 = BoundingBox()
box2.header.stamp = rospy.Time.now()
box2.header.frame_id = "camera_link"
box2.pose.orientation.w = 1
box2.pose.position.x = 0.8 * math.cos(theta)
box2.pose.position.y = 0.8 * math.sin(theta)
box2.pose.position.z = -0.1
box2.dimensions.x = 0.1
box2.dimensions.y = 0.1
box2.dimensions.z = 0.1
boxes.boxes = [box, box2]
boxes.header.frame_id = "camera_link"
boxes.header.stamp = rospy.Time.now()
pub.publish(boxes)
r.sleep()
r = rospy.Rate(100) # BODY filter body_bbox_msg = BoundingBox() body_bbox_msg.header.frame_id = 'BODY' body_bbox_msg.pose.position = Point(0.0, 0.0, 0.0) body_bbox_msg.pose.orientation = Quaternion(0.0, 0.0, 0.0, 0.0) body_bbox_msg.dimensions = Vector3(0.8, 1.6, 1.6) # Right hand filter rh_bbox_msg = BoundingBox() rh_bbox_msg.header.frame_id = 'R_thk_palm' rh_bbox_msg.pose.position = Point(0.15, 0.0, 0.0) rh_bbox_msg.pose.orientation = Quaternion(0.0, 0.0, 0.0, 0.0) rh_bbox_msg.dimensions = Vector3(0.3, 0.3, 0.15) # Left hand filter lh_bbox_msg = BoundingBox() lh_bbox_msg.header.frame_id = 'L_thk_palm' lh_bbox_msg.pose.position = Point(0.15, 0.0, 0.0) lh_bbox_msg.pose.orientation = Quaternion(0.0, 0.0, 0.0, 0.0) lh_bbox_msg.dimensions = Vector3(0.3, 0.3, 0.15) # make bbox array bbox_array_msg = BoundingBoxArray() bbox_array_msg.boxes = [body_bbox_msg, rh_bbox_msg, lh_bbox_msg] while not rospy.is_shutdown(): p.publish(bbox_array_msg) r.sleep()
r = rospy.Rate(10)
theta = 0
while not rospy.is_shutdown():
boxes = BoundingBoxArray()
theta = math.fmod(theta + 0.1, math.pi * 2)
box = BoundingBox()
box.header.stamp = rospy.Time.now()
box.header.frame_id = "velo_link"
box.pose.orientation.w = 1
box.pose.position.x = 0.8 * math.cos(theta)
box.pose.position.y = 0.8 * math.sin(theta)
box.pose.position.z = 0.1
box.dimensions.x = 0.1
box.dimensions.y = 0.1
box.dimensions.z = 0.1
box2 = BoundingBox()
box2.header.stamp = rospy.Time.now()
box2.header.frame_id = "velo_link"
box2.pose.orientation.w = 1
box2.pose.position.x = 0.8 * math.cos(theta)
box2.pose.position.y = 0.8 * math.sin(theta)
box2.pose.position.z = -0.1
box2.dimensions.x = 0.1
box2.dimensions.y = 0.1
box2.dimensions.z = 0.1
boxes.boxes = [box, box2]
boxes.header.frame_id = "velo_link"
boxes.header.stamp = rospy.Time.now()
pub.publish(boxes)
r.sleep()
def rslidar_callback(msg):
t_t = time.time()
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)
MarkerArray_list = MarkerArray() ##CREO EL MENSAJE GENERAL
if scores.size != 0:
for i in range(scores.size):
if scores[i] > 0.4:
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]) - 69.12 / 2
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)
obj = Marker()
#obj.CUBE = 1
obj.header.stamp = rospy.Time.now()
obj.header.frame_id = msg.header.frame_id
obj.type = Marker.CUBE
obj.id = i
obj.lifetime = rospy.Duration.from_sec(1)
#obj.type = int(types[i])
obj.pose.position.x = float(dt_box_lidar[i][0]) - 69.12 / 2
obj.pose.position.y = float(dt_box_lidar[i][1])
obj.pose.position.z = float(dt_box_lidar[i][2])
q = yaw2quaternion(float(dt_box_lidar[i][6]))
obj.pose.orientation.x = q[1]
obj.pose.orientation.y = q[2]
obj.pose.orientation.z = q[3]
obj.pose.orientation.w = q[0]
obj.scale.x = float(dt_box_lidar[i][3])
obj.scale.y = float(dt_box_lidar[i][4])
obj.scale.z = float(dt_box_lidar[i][5])
obj.color.r = 255
obj.color.a = 0.5
MarkerArray_list.markers.append(obj)
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)
pubMarker.publish(MarkerArray_list)
def merge_boxes(self, mask_rcnn_boxes, qatm_boxes, red_boxes):
boxes = BoundingBoxArray()
boxes.header = self.header
boxes.boxes = mask_rcnn_boxes.boxes + qatm_boxes.boxes + red_boxes.boxes
return boxes
def velo_callback(msg):
global proc
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)
print("np_p shape: ", np_p.shape)
#np_p = np.delete(np_p, -1, 1) # delete "ring" field
# convert to xyzi point cloud
x = np_p[:, 0].reshape(-1)
y = np_p[:, 1].reshape(-1)
z = np_p[:, 2].reshape(-1)
if np_p.shape[1] == 4: # if intensity field exists
i = np_p[:, 3].reshape(-1)
else:
i = np.zeros((np_p.shape[0], 1)).reshape(-1)
cloud = np.stack((x, y, z, i)).T
# start processing
dt_boxes_corners, scores, dt_box_lidar = proc.run(cloud)
# print(scores)
# # field of view cut
# cond = hv_in_range(x=np_p[:, 0],
# y=np_p[:, 1],
# z=np_p[:, 2],
# fov=[-45, 45],
# fov_type='h')
# cloud_ranged = cloud[cond]
# # print(cond.shape, np_p.shape, cloud.shape)
# publish to /velodyne_poitns_modified
# publish_test(cloud_ranged, msg.header.frame_id)
publish_test(proc.inputs['points'][0], msg.header.frame_id)
# process results
if scores.size != 0:
# print('Number of detections: ', results['name'].size)
for i in range(scores.size):
bbox = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
# bbox.header.stamp = rospy.Time.now()
q = quaternion_from_euler(0, 0, -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])
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:
# for i in range(0, len(arr_bbox.boxes)):
# print("[+] [x,y,z,dx,dy,dz] : {}, {}, {}, {}, {}, {}".\
# format(arr_bbox.boxes[i].pose.position.x,arr_bbox.boxes[i].pose.position.y,arr_bbox.boxes[i].pose.position.z,\
# arr_bbox.boxes[i].dimensions.x,arr_bbox.boxes[i].dimensions.y,arr_bbox.boxes[i].dimensions.z))
# publish to /voxelnet_arr_bbox
pub_arr_bbox.publish(arr_bbox)
#arr_bbox.boxes.clear()
arr_bbox.boxes = []
bb.dimensions.x = bb.dimensions.x + margin_x
bb.dimensions.y = bb.dimensions.y + margin_y
bb.dimensions.z = bb.dimensions.z + margin_z
bba.boxes = [bb]
pub.publish(bba)
if __name__ == '__main__':
rospy.init_node('pub_boundingbox_for_scan')
pub = rospy.Publisher("~box", BoundingBoxArray)
bba = BoundingBoxArray()
bb = BoundingBox()
r = rospy.Rate(1)
while not rospy.is_shutdown():
bb.header.stamp = rospy.Time.now()
bb.header.frame_id = "/map"
bb.pose.position.x = 3.7
bb.pose.position.y = 8.3
bb.pose.position.z = 0.3
bb.pose.orientation.x = 0
bb.pose.orientation.y = 0
bb.pose.orientation.z = 0
bb.pose.orientation.w = 1
bb.dimensions.x = 1.8
bb.dimensions.y = 2.0
bb.dimensions.z = 0.2
bba.boxes = [bb]
bba.header = bb.header
pub.publish(bba)
r.sleep()
bba = BoundingBoxArray()
bbapub = rospy.Publisher('boundingboxarray', BoundingBoxArray)
box.dimensions.x = 0.1
box.dimensions.y = 1
box.dimensions.z = 1
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('/map',
'/r_gripper_palm_link',
rospy.Time(0))
# import ipdb; ipdb.set_trace();
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rospy.loginfo("trans:%s rot:%s", trans, rot)
box.header.stamp = rospy.Time.now()
box.header.frame_id = "/map"
box.pose.position = Point(*trans)
print box.pose.position
box.pose.position.x += 0
box.pose.position.y += (box.dimensions.y / 2)
box.pose.position.z += (box.dimensions.z / 2)
print box.pose.position
# box.pose.position = Point(x=trans[0], y=trans[1], z=trans[2])
box.pose.orientation = Quaternion(*rot)
print box
bba.header = box.header
bba.boxes = [box]
bbapub.publish(bba)
rate.sleep()
def velo_callback(msg):
global proc
global seq_num
time_start = rospy.get_rostime()
callback_time = rospy.get_rostime() - msg.header.stamp
callback_time_secs = callback_time.secs + callback_time.nsecs/1e9
print("Time between message publish and callback:", callback_time_secs, "seconds")
arr_bbox = BoundingBoxArray()
arr_dobject = DetectedObjectArray()
#pcl_msg = pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z","intensity","ring"))
#pcl_msg = pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z"))
#np_p = np.array(list(pcl_msg), dtype=np.float32)
np_p = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
#print("np_p shape: ", np_p.shape)
#np_p = np.delete(np_p, -1, 1) # delete "ring" field
# convert to xyzi point cloud
x = np_p[:, 0].reshape(-1)
y = np_p[:, 1].reshape(-1)
z = np_p[:, 2].reshape(-1)
if np_p.shape[1] == 4: # if intensity field exists
i = np_p[:, 3].reshape(-1)
else:
i = np.zeros((np_p.shape[0], 1)).reshape(-1)
cloud = np.stack((x, y, z, i)).T
#print("cloud[0]:",cloud[0,0])
# start processing
dt_boxes_corners, scores, dt_box_lidar, label = proc.run(cloud)
#print("dt_boxes_corners :",dt_boxes_corners)
# print(scores)
# # field of view cut
cond = hv_in_range(x=np_p[:, 0],
y=np_p[:, 1],
z=np_p[:, 2],
fov=[-90, 90],
fov_type='h')
cloud_ranged = cloud[cond]
cloud_ranged_pc2 = xyz_array_to_pointcloud2(cloud_ranged[:,:3],frame_id="velodyne") ##pay attention here, xyz_array_to_pointcloud2 only works for nX3 array!!!
# # print(cond.shape, np_p.shape, cloud.shape)
# publish to /velodyne_poitns_modified
# publish_test(cloud_ranged, msg.header.frame_id)
#publish_test(proc.inputs['points'][0], msg.header.frame_id)
#print("shape for points_display", points_display[0].shape)
#publish_test(points_display[0], msg.header.frame_id)
label_n = []
for la,la2 in enumerate(label):
#print("la is:",la)
#print("la2 is:",la2)
if la2 == 'Car':
label_n.append(1)
elif la2 == 'Cyclist':
label_n.append(2)
elif la2 == 'Pedestrian':
label_n.append(3)
else:
label_n.append(4)
# process results
#print("label_n is ",label_n)
end_time_1 = rospy.get_rostime() - time_start
end_time_1_sec = end_time_1.secs + end_time_1.nsecs/1e9
print("end_time_1 is :",end_time_1_sec)
segment_index = box_np_ops.points_in_rbbox(cloud, dt_box_lidar, lidar=True)
end_time_2 = rospy.get_rostime() - time_start
end_time_2_sec = end_time_2.secs + end_time_2.nsecs/1e9
print("end_time_2 is :",end_time_2_sec)
#print("shape of segment_index is",segment_index.shape)
#print("segment_index is", segment_index)
#print(type(cloud))
#segment_cloud = np.matmul(cloud.T,segment_index)
segment_cloud_agg = np.array([])
seq_num = seq_num + 1
if scores.size != 0:
# print('Number of detections: ', results['name'].size)
for i in range(scores.size):
bbox = BoundingBox()
dobject = DetectedObject()
#DetectedObject, DetectedObjectArray
#box_test = BoundingBox()
bbox.header.frame_id = msg.header.frame_id
dobject.header.frame_id = msg.header.frame_id
#box_test.header.frame_id = msg.header.frame_id
# bbox.header.stamp = rospy.Time.now()
q = quaternion_from_euler(0,0,-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]+dt_box_lidar[i][5]/2)
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.label = label_n[i]
dobject.header = msg.header
dobject.header.seq = seq_num
#dobject.id = i
#dobject.header.label = label[i]
#dobject.header.score = scores[i]
dobject.label = label[i]
dobject.score = 1.
dobject.space_frame = "velodyne"
dobject.pose = bbox.pose
dobject.dimensions = bbox.dimensions
segment_cloud = cloud[segment_index[:,i],:3] #pay attention here, xyz_array_to_pointcloud2 only works for nX3 array!!!
#print("segment_cloud",segment_cloud)
#print("shape_segment_pc:", segment_cloud.shape)
segment_cloud_pc2 = xyz_array_to_pointcloud2(segment_cloud)
#segment_cloud = cloud
#segment_cloud_pc2 = pc2.create_cloud(header=msg.header,
# fields=_make_point_field(4), # 4 PointFields as channel description
# points=segment_cloud)
#dobject.pointcloud = segment_cloud_pc2
dobject.valid = True
#print("shape of segment_cloud",segment_cloud.shape)
#if segment_cloud_agg.size == 0:
# segment_cloud_agg = segment_cloud
#else:
# segment_cloud_agg = np.append(segment_cloud_agg,segment_cloud,axis=0)
#box_test.pose.orientation.x = q[0]
#box_test.pose.orientation.x = q[1]
#box_test.pose.orientation.x = q[2]
#box_test.pose.orientation.x = q[3]
#box_test.pose.position.x = 5
#box_test.pose.position.y = 0
#box_test.pose.position.z = -1
#box_test.dimensions.x = float(dt_box_lidar[i][3])
#box_test.dimensions.y = float(dt_box_lidar[i][4])
#box_test.dimensions.z = float(dt_box_lidar[i][5])
arr_bbox.boxes.append(bbox)
arr_dobject.objects.append(dobject)
arr_bbox.header.frame_id = msg.header.frame_id
arr_dobject.header = msg.header
arr_dobject.header.frame_id = msg.header.frame_id
#arr_dobject.header = 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:
# for i in range(0, len(arr_bbox.boxes)):
# print("[+] [x,y,z,dx,dy,dz] : {}, {}, {}, {}, {}, {}".\
# format(arr_bbox.boxes[i].pose.position.x,arr_bbox.boxes[i].pose.position.y,arr_bbox.boxes[i].pose.position.z,\
# arr_bbox.boxes[i].dimensions.x,arr_bbox.boxes[i].dimensions.y,arr_bbox.boxes[i].dimensions.z))
# publish to /voxelnet_arr_bbox
end_time_3 = rospy.get_rostime() - time_start
end_time_3_sec = end_time_3.secs + end_time_3.nsecs/1e9
print("end_time_3 is :",end_time_3_sec)
pub_arr_bbox.publish(arr_bbox)
pub_arr_dobject.publish(arr_dobject)
pub_ranged_cloud.publish(cloud_ranged_pc2)
#print("size of segment_cloud_agg",segment)
#publish_segment(segment_cloud_agg,msg.header.frame_id)
#pub_test_box.publish(box_test)
#arr_bbox.boxes.clear()
arr_bbox.boxes = []
arr_dobject.objects = []
