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)