def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame='/map')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(
msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(
position_np - self.position_offset, orientation_np)
self.state_pub.publish(header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
header = sub8_utils.make_header(frame='/world')
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
dist = np.linalg.norm(
sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame="/map")
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(position_np - self.position_offset, orientation_np)
self.state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
header = sub8_utils.make_header(frame="/world")
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
dist = np.linalg.norm(sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
def find_marker(self, img):
#img[:, -100:] = 0
img = cv2.GaussianBlur(img, (7, 7), 15)
last_image_timestamp = self.last_image_timestamp
if rospy.get_param("/orange_markers/use_boost"):
some_observations = machine_learning.boost.observe(img)
prediction = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
mask = np.reshape(prediction, img[:, :, 2].shape).astype(np.uint8) * 255
else:
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.lower, self.upper)
kernel = np.ones((5,5),np.uint8)
mask = cv2.dilate(mask, kernel, iterations = 1)
mask = cv2.erode(mask, kernel, iterations = 2)
#mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
contours, _ = cv2.findContours(np.copy(mask), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) < 1:
rospy.logwarn("MARKER - No marker found.")
return None
# Find biggest area contour
self.last_draw_image = np.dstack([mask] * 3)
areas = [cv2.contourArea(c) for c in contours]
max_index = np.argmax(areas)
cnt = contours[max_index]
# Draw a miniAreaRect around the contour and find the area of that.
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
mask = np.zeros(shape=mask.shape)
cv2.drawContours(self.last_draw_image, [box], 0, (0, 128, 255), 2)
cv2.drawContours(mask, [box], 0, 255, -1)
rect_area = cv2.contourArea(box)
center, angle_rad, [max_eigv, min_eigv] = self.get_2d_pose(mask)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * max_eigv))), (0, 255, 30), 2)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * min_eigv))), (0, 30, 255), 2)
# Check if the box is too big or small.
xy_position, height = self.get_tf(timestamp=last_image_timestamp)
expected_area = self.calculate_marker_area(height)
print "{} < {} < {} ({})".format(expected_area * .2, rect_area, expected_area * 2, expected_area)
if expected_area * .5 < rect_area < expected_area * 2:
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 255, 255), -1)
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
else:
angle_rad = 0
max_eigv = np.array([0, -20])
min_eigv = np.array([-20, 0])
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 0, 30), -1)
rospy.logwarn("MARKER - Size out of bounds!")
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
# Convert to a 3d pose to move the sub to.
abs_position = self.transform_px_to_m(center, last_image_timestamp)
q_rot = tf.transformations.quaternion_from_euler(0, 0, angle_rad)
return numpy_quat_pair_to_pose(abs_position, q_rot)
