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 = []