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 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(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 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 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 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 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 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 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 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 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 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 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 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 _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 prepareDetectionRegistration(self, centroid, now):
obj_det = BucketDetection()
obj_det.image = self.cv_bridge.cv2_to_imgmsg(self.stereo_left, "bgr8")
obj_det.tag = "object_tags/gate"
bbox_3d = BoundingBox()
bbox_3d.dimensions = Vector3(self.gate_dimensions[0], self.gate_dimensions[1], self.gate_dimensions[2])
bbox_pose = Pose()
x, y, z = list((np.squeeze(centroid)).T)
obj_det.position = Point(x, y, z)
bbox_pose.position = Point(x, y, z)
bbox_3d.pose = bbox_pose
bbox_header = Header()
bbox_header.frame_id = "duo3d_optical_link_front"
bbox_header.stamp = now
bbox_3d.header = bbox_header
obj_det.bbox_3d = bbox_3d
obj_det.header = Header()
obj_det.header.frame_id = bbox_header.frame_id
obj_det.header.stamp = now
return obj_det
def prepare_detection_registration(self, centroid, det, now):
obj_det = BucketDetection()
obj_det.image = self.cv_bridge.cv2_to_imgmsg(self.stereo_left, "bgr8")
obj_det.tag = str("object_tags/" + self.classes[det[0]])
bbox_dims = np.asarray([1.0, 1.0, 1.0])
if rospy.has_param("object_tags/" + self.classes[det[0]] + "/dimensions"):
bbox_dims = np.asarray(rospy.get_param("object_tags/" + self.classes[det[0]] + "/dimensions")).astype(float)
bbox_3d = BoundingBox()
bbox_3d.dimensions = Vector3(bbox_dims[0], bbox_dims[1], bbox_dims[2])
bbox_pose = Pose()
x, y, z = list((np.squeeze(centroid)).T)
obj_det.position = Point(x, y, z)
bbox_pose.position = Point(x, y, z)
bbox_3d.pose = bbox_pose
bbox_header = Header()
bbox_header.frame_id = "duo3d_optical_link_front"
bbox_header.stamp = now
bbox_3d.header = bbox_header
obj_det.bbox_3d = bbox_3d
obj_det.header = Header()
obj_det.header.frame_id = bbox_header.frame_id
obj_det.header.stamp = now
return obj_det
def callback(self, msg):
target_frame = self.frame_id
input_frame = msg.header.frame_id
transform = self.tf_buffer.lookup_transform(
target_frame,
input_frame, #source frame
rospy.Time(0),
rospy.Duration(1.0))
print("pose array clipper callback")
posearray = msg.poses
x_min = self.initial_pos[0] - self.dimension_x / 2
x_max = self.initial_pos[0] + self.dimension_x / 2
y_min = self.initial_pos[1] - self.dimension_y / 2
y_max = self.initial_pos[1] + self.dimension_y / 2
z_min = self.initial_pos[2] - self.dimension_z / 2
z_max = self.initial_pos[2] + self.dimension_z / 2
pub_msg = PoseArray()
for input_pose in posearray:
pose_stamped = PoseStamped()
pose_stamped.pose = input_pose
tf_pose = tf2_geometry_msgs.do_transform_pose(
pose_stamped, transform)
x = tf_pose.pose.position.x
y = tf_pose.pose.position.y
z = tf_pose.pose.position.z
if (x >= x_min) and (x <= x_max):
if (y >= y_min) and (y <= y_max):
if (z >= z_min) and (z <= z_max):
pub_msg.poses.append(tf_pose.pose)
pub_msg.header.frame_id = self.frame_id
self.pub_output_poses.publish(pub_msg)
bbox = BoundingBox()
pose = Pose()
pose.position.x = self.initial_pos[0]
pose.position.y = self.initial_pos[1]
pose.position.z = self.initial_pos[2]
bbox.pose = pose
vec = Vector3()
vec.x = self.dimension_x
vec.y = self.dimension_y
vec.z = self.dimension_z
bbox.dimensions = vec
bbox.header.frame_id = self.frame_id
self.pub_clipper_bbox.publish(bbox)
def od_msg_callback(self, od, classes):
bboxes = BoundingBoxArray(header=od.header)
for obj in od.objects:
b = BoundingBox()
b.pose = obj.pose
self.box_msg_callback(bboxes, classes)
def get_bbox_pose(self, bbox, cloud):
# Initialize variables
n = 0
cog = [0] * 3
min_dim = [None] * 3
max_dim = [None] * 3
cloud_dim = [0] * 3
# Get the width and height of the bbox
width = bbox[2] - bbox[0]
height = bbox[3] - bbox[1]
# Shrink the bbox by 10% to minimize background points
xmin = bbox[0] + int(0.1 * width)
xmax = bbox[2] - int(0.1 * width)
ymin = bbox[1] + int(0.1 * height)
ymax = bbox[3] - int(0.1 * height)
# Place all the box points in an array to be used by the read_points function below
bbox_points = [[x, y] for x in range(xmin, xmax)
for y in range(ymin, ymax)]
# Clear the current list
person_points = list()
# Read in the x, y, z coordinates of all bbox points in the cloud
for point in point_cloud2.read_points(cloud,
skip_nans=True,
uvs=bbox_points):
try:
for i in range(3):
if min_dim[i] is None or point[i] < min_dim[i]:
min_dim[i] = point[i]
if max_dim[i] is None or point[i] > max_dim[i]:
max_dim[i] = point[i]
cog[i] += point[i]
n += 1
#person_points.append(point)
except:
pass
# Publish the pointcloud for this person
#person_cloud = point_cloud2.create_cloud(cloud.header, cloud.fields, person_points)
#self.people_cloud_pub.publish(person_cloud)
# If we don't have any points, just return empty handed
if n == 0:
return (None, None)
# Compute the COG and dimensions of the bounding box
for i in range(3):
cog[i] /= n
cloud_dim[i] = max_dim[i] - min_dim[i]
# Fill in the person pose message
person_cog = PoseStamped()
person_cog.header.frame_id = self.depth_frame
person_cog.header.stamp = rospy.Time.now()
person_cog.pose.position.x = cog[0]
person_cog.pose.position.y = cog[1]
person_cog.pose.position.z = cog[2]
# Project the COG onto the base frame
try:
person_in_base_frame = self.tfBuffer.transform(
person_cog, self.base_link)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
return (None, None)
# Set the person on the ground
person_in_base_frame.pose.position.z = 0.0
person_in_base_frame.pose.orientation.x = 0.707
person_in_base_frame.pose.orientation.y = 0.0
person_in_base_frame.pose.orientation.z = 0.707
person_in_base_frame.pose.orientation.w = 0.0
# Fill in the BoundBox message
person_bounding_box = BoundingBox()
person_bounding_box.header.frame_id = self.depth_frame
person_bounding_box.header.stamp = rospy.Time.now()
person_bounding_box.pose = person_in_base_frame.pose
person_bounding_box.dimensions.x = cloud_dim[0]
person_bounding_box.dimensions.y = cloud_dim[1]
person_bounding_box.dimensions.z = cloud_dim[2]
return (person_in_base_frame, person_bounding_box)
