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)