def image_callback(self, img):
try:
cv_image = CvBridge().imgmsg_to_cv2(img, "bgr8")
pil_image = Image.fromarray(
cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB))
width, height = pil_image.size
resize_ratio = 1.0 * self.input_size / max(width, height)
target_size = (int(resize_ratio * width),
int(resize_ratio * height))
resized_image = pil_image.convert('RGB').resize(
target_size, Image.ANTIALIAS)
input_image = resized_image.to(self.device)
with torch.no_grad():
output = self.model(input_image)
seg_image = np.squeeze(output.data.max(1)[1].cpu().numpy(), axis=0)
seg_image = decode_segmap(self.num_classes, seg_image)
cv_seg_image = np.asarray(seg_image)
pub_seg_image = CvBridge().cv2_to_imgmsg(cv_seg_image, "rgb8")
pub_seg_image.header = img.header
if not self.use_subscribed_images_stamp:
pub_seg_image.header.stamp = rospy.get_rostime()
self.segmented_image_pub.publish(pub_seg_image)
except CvBridgeError as e:
print(e)
def callback(data):
print(rospy.get_name(), "I heard %s" % str(data.data))
img_raveled = data.data[0:-2]
img_size = data.data[-2:].astype(int)
img = np.float32(np.reshape(img_raveled, (img_size[0], img_size[1])))
#img = np.float32((np.reshape(data.data, (DEPTH_IMG_WIDTH, DEPTH_IMG_HEIGHT))))
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
image_message = CvBridge().cv2_to_imgmsg(img, encoding="passthrough")
image_message.header = h
pub.publish(image_message)
camera_info_msg = CameraInfo()
camera_info_msg.header = h
fx, fy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2
cx, cy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2
camera_info_msg.width = DEPTH_IMG_WIDTH
camera_info_msg.height = DEPTH_IMG_HEIGHT
camera_info_msg.distortion_model = "plumb_bob"
camera_info_msg.K = np.float32([fx, 0, cx, 0, fy, cy, 0, 0, 1])
camera_info_msg.D = np.float32([0, 0, 0, 0, 0])
camera_info_msg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]
camera_info_pub.publish(camera_info_msg)
def load_and_publish_images(self, annotation_filename):
try:
f = open(annotation_filename)
annotation = json.loads(f.read())
ids = annotation['class_id']
bboxes = annotation['bbox']
image = cv2.imread(
self._data_dir + '/Annotations/' + annotation['img_path'],
cv2.IMREAD_UNCHANGED)
for id, bbox in zip(ids, bboxes):
self.draw_bbox(image, id, bbox)
imgmsg = CvBridge().cv2_to_imgmsg(image, encoding='passthrough')
depth = cv2.imread(
self._data_dir + '/Annotations/' + annotation['depth_path'],
cv2.IMREAD_UNCHANGED)
dptmsg = CvBridge().cv2_to_imgmsg(depth, encoding='passthrough')
except Exception as e:
rospy.logerr('%s(%s)', str(e), annotation_filename)
return
for bbox in bboxes:
self._dfilter.roi = bbox
now = rospy.Time.now()
self._cinfo.header.stamp = now
imgmsg.header = self._cinfo.header
dptmsg.header = self._cinfo.header
self._cinfo_pub.publish(self._cinfo)
self._image_pub.publish(imgmsg)
self._depth_pub.publish(dptmsg)
key = raw_input('Hit [p|q|return] >> ')
if key == 'p':
self._dfilter.detect_plane_send_goal()
plane = self._dfilter.detect_plane_wait_for_result()
print(plane)
elif key == 'q':
sys.exit()
def load_and_localize(self, annotation_filename):
self._spawner.delete_all()
try:
f = open(annotation_filename)
annotation = json.loads(f.read())
ids = annotation['class_id']
bboxes = annotation['bbox']
image = cv2.imread(
self._data_dir + '/Annotations/' + annotation['img_path'],
cv2.IMREAD_UNCHANGED)
for id, bbox in zip(ids, bboxes):
self.draw_bbox(image, id, bbox)
imsg = CvBridge().cv2_to_imgmsg(image, encoding='passthrough')
depth = cv2.imread(
self._data_dir + '/Annotations/' + annotation['depth_path'],
cv2.IMREAD_UNCHANGED)
dmsg = CvBridge().cv2_to_imgmsg(depth, encoding='passthrough')
except Exception as e:
rospy.logerr('(Feeder) %s(%s)', str(e), annotation_filename)
return
for id, bbox in zip(ids, bboxes):
self._dfilter.roi = bbox
now = rospy.Time.now()
self._cinfo.header.stamp = now
imsg.header = self._cinfo.header
dmsg.header = self._cinfo.header
self._cinfo_pub.publish(self._cinfo)
self._image_pub.publish(imsg)
self._depth_pub.publish(dmsg)
rospy.loginfo('*** (Feeder) --------------')
rospy.loginfo('*** (Feeder) localize id=%d', id + 1)
self.localize(ImageFeeder._Models[id])
def image_callback(self, img: Image):
t_start = time.time()
if not self.camera.ready(
) or not self.trajectory_complete or not self.goal_post_need:
return
self.camera.reset_position(publish_basecamera=False,
timestamp=img.header.stamp)
image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8")
hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV)
h = self.camera.calculateHorizonCoverArea()
cv2.rectangle(image, [0, 0], [640, int(h * 7 / 10.0)], [0, 0, 0],
cv2.FILLED)
# Field line detection
mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255))
out = cv2.bitwise_and(image, image, mask=mask2)
kernel = np.ones((7, 7), np.uint8)
out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel)
cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY)
retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY)
edges = cv2.Canny(dst, 50, 150)
lines = cv2.HoughLinesP(
edges,
rho=DetectorFieldline.HOUGH_RHO,
theta=DetectorFieldline.HOUGH_THETA,
threshold=DetectorFieldline.HOUGH_THRESHOLD,
minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH,
maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP)
ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB)
if lines is None:
return
computed_lines = []
vert_x_max, vert_x_min, vert_y_max, vert_y_min = 0, 1000, 0, 1000
for l in lines:
x1, y1, x2, y2 = l[0]
# computing magnitude and angle of the line
mag = np.sqrt((x2 - x1)**2. + (y2 - y1)**2.)
if x2 == x1:
angle = 0
else:
angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1)))
computed_lines += [(mag, angle)]
pt1 = (x1, y1)
pt2 = (x2, y2)
if abs(angle) < 10: # Horizontal
cv2.line(ccdst,
pt1,
pt2, (255, 0, 0),
thickness=3,
lineType=cv2.LINE_AA)
elif abs(abs(angle) - 90) < 10: # Vertical
cv2.line(ccdst,
pt1,
pt2, (0, 255, 0),
thickness=3,
lineType=cv2.LINE_AA)
vert_x_max = max(vert_x_max, x1, x2)
vert_x_min = min(vert_x_min, x1, x2)
vert_y_max = max(vert_y_max, y1, y2)
vert_y_min = min(vert_y_min, y1, y2)
else:
cv2.line(ccdst,
pt1,
pt2, (0, 0, 255),
thickness=3,
lineType=cv2.LINE_AA)
computed_lines = np.array(computed_lines)
# an image has a goalpost if two perpendicular lines with 0 degrees and 90 degrees intersect
vertical_line = len(
computed_lines[(abs(abs(computed_lines[:, 1]) - 90) < 10)]) > 0
if vertical_line:
w = vert_y_max - vert_y_min
l = vert_x_max - vert_x_min
area = w * l
if area < 50000:
cv2.rectangle(ccdst, [vert_x_min, vert_y_min],
[vert_x_max, vert_y_max], [0, 255, 255],
thickness=2)
x_avg = (vert_x_max + vert_x_min) / 2
[floor_center_x, floor_center_y,
_] = self.camera.findFloorCoordinate([x_avg, vert_y_max])
[floor_close_x, floor_close_y,
_] = self.camera.findFloorCoordinate([x_avg, vert_y_max])
camera_pose = self.camera.pose
distance = (
(floor_center_x - camera_pose.get_position()[0])**2 +
(floor_center_y - camera_pose.get_position()[1])**2)**0.5
theta = math.atan2(distance, camera_pose.get_position()[2])
ratio = math.tan(theta)**2
ratio2 = 1 / (1 + ratio)
if 1 < ratio2 < 0:
print('here') # TODO
floor_x = floor_close_x * (1 -
ratio2) + floor_center_x * ratio2
floor_y = floor_close_y * (1 -
ratio2) + floor_center_y * ratio2
if floor_x > 0.0:
br = tf2_ros.TransformBroadcaster()
robot_pose = TransformStamped()
robot_pose.header.frame_id = self.robot_name + "/base_camera"
robot_pose.child_frame_id = self.robot_name + "/goal_post"
robot_pose.header.stamp = img.header.stamp
robot_pose.header.seq = img.header.seq
robot_pose.transform.translation.x = floor_x
robot_pose.transform.translation.y = floor_y
robot_pose.transform.translation.z = 0
robot_pose.transform.rotation.x = 0
robot_pose.transform.rotation.y = 0
robot_pose.transform.rotation.z = 0
robot_pose.transform.rotation.w = 1
br.sendTransform(robot_pose)
if self.image_publisher.get_num_connections() > 0:
img_out = CvBridge().cv2_to_imgmsg(ccdst)
img_out.header = img.header
self.image_publisher.publish(img_out)
t_end = time.time()
rospy.loginfo_throttle(
20, "GoalPost detection rate: " + str(t_end - t_start))
def image_callback(self, img: Image):
rospy.loginfo_once("Recieved Message")
t_start = time.time()
if not self.camera.ready() or not self.trajectory_complete:
return
pts = []
self.camera.reset_position(publish_basecamera=True, timestamp=img.header.stamp)
image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8")
hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV)
h = self.camera.calculateHorizonCoverArea()
cv2.rectangle(image, [0, 0], [640, h], [0, 0, 0], cv2.FILLED)
# Field line detection
mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255))
out = cv2.bitwise_and(image, image, mask=mask2)
kernel = np.ones((7, 7), np.uint8)
out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel)
cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY)
retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY)
edges = cv2.Canny(dst, 50, 150)
lines = cv2.HoughLinesP(edges, rho=DetectorFieldline.HOUGH_RHO,
theta=DetectorFieldline.HOUGH_THETA,
threshold=DetectorFieldline.HOUGH_THRESHOLD,
minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH,
maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP)
ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB)
if lines is None:
return
for l in lines:
x1, y1, x2, y2 = l[0]
# computing magnitude and angle of the line
if x2 == x1:
angle = 0
else:
angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1)))
pt1 = (x1, y1)
pt2 = (x2, y2)
if abs(angle) < 10: # Horizontal
cv2.line(ccdst, pt1, pt2, (255, 0, 0), thickness=3, lineType=cv2.LINE_AA)
elif abs(abs(angle) - 90) < 10: # Vertical
cv2.line(ccdst, pt1, pt2, (0, 255, 0), thickness=3, lineType=cv2.LINE_AA)
else:
cv2.line(ccdst, pt1, pt2, (0, 0, 255), thickness=3, lineType=cv2.LINE_AA)
if (pt2[0] - pt1[0]) == 0:
continue
slope = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0])
b = y1 - slope * x1
for i in range(x1, x2, 15):
y = slope * i + b
pt = [i, y]
pts.append(pt)
if self.image_publisher.get_num_connections() > 0:
img_out = CvBridge().cv2_to_imgmsg(ccdst)
img_out.header = img.header
self.image_publisher.publish(img_out)
if self.point_cloud_publisher.get_num_connections() > 0:
points3d = []
for p in pts:
points3d.append(self.camera.findFloorCoordinate(p))
# Publish fieldlines in laserscan format
header = Header()
header.stamp = img.header.stamp
header.frame_id = self.robot_name + "/base_camera"
point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d)
if self.point_cloud_publisher.get_num_connections() > 0:
self.point_cloud_publisher.publish(point_cloud_msg)
t_end = time.time()
rospy.loginfo_throttle(20, "Fieldline detection rate: " + str(t_end - t_start))
