def pubCorners(self, args=None):
self.pub_camera_info.publish(self.camera_info_msg)
corners_msg_out = VehicleCorners()
corners_msg_out.header.stamp = rospy.Time.now()
corners_msg_out.H = 3
corners_msg_out.W = 7
corners_msg_out.detection.data = True
# p_1 = Point32()
# p_2 = Point32()
# p_3 = Point32()
#
# p_1.x = 0.0
# p_1.y = 10.0
#
# p_2.x = 1.0
# p_2.y = 11.0
#
# p_3.x = 2.0
# p_3.y = 12.0
#
# corners_msg_out.corners.append(p_1)
# corners_msg_out.corners.append(p_2)
# corners_msg_out.corners.append(p_3)
for i in range(0, 21):
point = Point32()
point.x = i * 2.0
point.y = i * 3.0
corners_msg_out.corners.append(point)
self.pub_corners.publish(corners_msg_out)
rospy.loginfo("Publishing corners")
def processImage(self, image_msg):
if not self.active:
return
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
vehicle_detected_msg_out = BoolStamped()
vehicle_corners_msg_out = VehicleCorners()
try:
image_cv = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print e
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
params.minArea = self.blobdetector_min_area
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
(detection, corners) = cv2.findCirclesGrid(image_cv,
self.circlepattern_dims, flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
# print(corners)
vehicle_detected_msg_out.data = detection
self.pub_detection.publish(vehicle_detected_msg_out)
if detection:
# print(corners)
points_list = []
for point in corners:
corner = Point32()
# print(point[0])
corner.x = point[0, 0]
# print(point[0,1])
corner.y = point[0, 1]
corner.z = 0
points_list.append(corner)
vehicle_corners_msg_out.header.stamp = rospy.Time.now()
vehicle_corners_msg_out.corners = points_list
vehicle_corners_msg_out.detection.data = detection
vehicle_corners_msg_out.H = self.circlepattern_dims[1]
vehicle_corners_msg_out.W = self.circlepattern_dims[0]
self.pub_corners.publish(vehicle_corners_msg_out)
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
if self.publish_circles:
cv2.drawChessboardCorners(image_cv,
self.circlepattern_dims, corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
def cb_image(self, image_msg):
"""
Callback for processing a image which potentially contains a back pattern. Processes the image only if
sufficient time has passed since processing the previous image (relative to the chosen processing frequency).
The pattern detection is performed using OpenCV's `findCirclesGrid <https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=solvepnp#findcirclesgrid>`_ function.
Args:
image_msg (:obj:`sensor_msgs.msg.CompressedImage`): Input image
"""
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
vehicle_centers_msg_out = VehicleCorners()
detection_flag_msg_out = BoolStamped()
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
(detection, centers) = cv2.findCirclesGrid(
image_cv,
patternSize=tuple(self.circlepattern_dims.value),
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=self.simple_blob_detector,
)
# if the pattern is detected, cv2.findCirclesGrid returns a non-zero result, otherwise it returns 0
# vehicle_detected_msg_out.data = detection > 0
# self.pub_detection.publish(vehicle_detected_msg_out)
vehicle_centers_msg_out.header = image_msg.header
vehicle_centers_msg_out.detection.data = detection > 0
detection_flag_msg_out.header = image_msg.header
detection_flag_msg_out.data = detection > 0
# if the detection is successful add the information about it,
# otherwise publish a message saying that it was unsuccessful
if detection > 0:
points_list = []
for point in centers:
center = Point32()
center.x = point[0, 0]
center.y = point[0, 1]
center.z = 0
points_list.append(center)
vehicle_centers_msg_out.corners = points_list
vehicle_centers_msg_out.H = self.circlepattern_dims.value[1]
vehicle_centers_msg_out.W = self.circlepattern_dims.value[0]
self.pub_centers.publish(vehicle_centers_msg_out)
self.pub_detection_flag.publish(detection_flag_msg_out)
if self.pub_circlepattern_image.get_num_connections() > 0:
cv2.drawChessboardCorners(image_cv,
tuple(self.circlepattern_dims.value),
centers, detection)
image_msg_out = self.bridge.cv2_to_compressed_imgmsg(image_cv)
self.pub_circlepattern_image.publish(image_msg_out)
def pubCorners(self, args=None):
corners_msg_out = VehicleCorners()
corners_msg_out.header.stamp = rospy.Time.now()
p_1 = Point32()
p_2 = Point32()
p_3 = Point32()
p_1.x = 0.0
p_1.y = 10.0
p_2.x = 1.0
p_2.y = 11.0
p_3.x = 2.0
p_3.y = 12.0
corners_msg_out.corners.append(p_1)
corners_msg_out.corners.append(p_2)
corners_msg_out.corners.append(p_3)
self.pub_corners.publish(corners_msg_out)
rospy.loginfo("Publishing corners")
def processImage(self, image_msg):
if not self.active:
return
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
vehicle_detected_msg_out = BoolStamped()
vehicle_corners_msg_out = VehicleCorners()
try:
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print e
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
# params.filterByArea = True
params.minArea = self.blobdetector_min_area
# params.minThreshold = 100
# params.maxThreshold = 255
# params.filterByCircularity = True
# params.minCircularity = 0.4
# params.filterByInertia = True
# params.minInertiaRatio = 0.6
# params.filterByConvexity = True
# params.minConvexity = 0.8
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
stay_detected = False
(detection,
corners) = cv2.findCirclesGrid(image_cv,
self.circlepattern_dims,
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
# print(detection)
# if not stay_detected:
# keypoints = simple_blob_detector.detect(image_cv)
# im_with_keypoints = cv2.drawKeypoints(image_cv, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
# self.pub_circlepattern_image.publish(self.bridge.cv2_to_imgmsg(im_with_keypoints, "bgr8"))
vehicle_detected_msg_out.data = detection
self.pub_detection.publish(vehicle_detected_msg_out)
if detection:
# print(corners)
points_list = []
for point in corners:
corner = Point32()
# print(point[0])
corner.x = point[0, 0]
# print(point[0,1])
corner.y = point[0, 1]
corner.z = 0
points_list.append(corner)
vehicle_corners_msg_out.header.stamp = rospy.Time.now()
vehicle_corners_msg_out.corners = points_list
vehicle_corners_msg_out.detection.data = detection
vehicle_corners_msg_out.H = self.circlepattern_dims[1]
vehicle_corners_msg_out.W = self.circlepattern_dims[0]
self.pub_corners.publish(vehicle_corners_msg_out)
if detection and self.publish_circles:
stay_detected = True
cv2.drawChessboardCorners(image_cv, self.circlepattern_dims,
corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
def processImage(self, image_msg):
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
self.loginfo("Processing image")
vehicle_detected_msg_out = BoolStamped()
vehicle_corners_msg_out = VehicleCorners()
try:
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print(e)
# TODO we are only finding a 3x1 circle grid
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
params.minArea = self.blobdetector_min_area
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
(detection,
corners) = cv2.findCirclesGrid(image_cv, (3, 1),
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
vehicle_detected_msg_out.data = detection
# if(vehicle_detected_msg_out.data is not False):
# self.loginfo(">>>>>>>>>>>>>>>>>>>>>Published on detection")
self.pub_detection.publish(vehicle_detected_msg_out)
if detection:
self.logwarn("Detected vehicle")
# print(corners)
points_list = []
for point in corners:
corner = Point32()
# print(point[0])
corner.x = point[0, 0]
# print(point[0,1])
corner.y = point[0, 1]
corner.z = 0
points_list.append(corner)
vehicle_corners_msg_out.header.stamp = rospy.Time.now()
vehicle_corners_msg_out.corners = points_list
vehicle_corners_msg_out.detection.data = detection
vehicle_corners_msg_out.H = self.circlepattern_dims[1]
vehicle_corners_msg_out.W = self.circlepattern_dims[0]
self.pub_corners.publish(vehicle_corners_msg_out)
# If we changed detection state
if (self.vehicle_detected != detection):
state = stop_fsm_state if detection else follow_fsm_state
# Set state to vehicle detected -> Stop
request = SetFSMStateRequest(state)
self.loginfo("Detection state to send: " + str(state))
try:
self.setFSMState(request)
except rospy.ServiceException as exc:
rospy.logwarn("FSM service did not process changeState stop:" +
str(exc))
except Exception as e:
rospy.logwarn(
"FSM service did not process changeState, exception: " +
str(e))
except:
rospy.logwarn("FSM service did not process changeState")
self.vehicle_detected = detection
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
if self.publish_circles:
cv2.drawChessboardCorners(image_cv, self.circlepattern_dims,
corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
return