def img_callback(self):
ret = msg.Image()
while (1):
if not self.cap.isOpened():
print("Camera can't open\nexit")
return -1
ret, img = self.cap.read()
img = cv2.cvtColor(img, cv2.COLOR_BayerGB2RGB)
img = cv2.resize(img, dsize=(args.width, args.height))
width, height, channels = img.shape
#print("width: %d, height: %d" %(width,height))
cv2.imshow("fuck_flir", img)
temp = CvBridge().cv2_to_imgmsg(img, encoding='bgr8')
#print(width, height)
'''
ret.data = temp.data
ret.encoding = temp.encoding
ret.header.frame_id = temp.header.frame_id
ret.step = temp.step
ret.height = height
ret.width = width
'''
self.publisher_.publish(temp)
key = cv2.waitKey(1)
if key == ord("q"):
break
def make_goal(self, ud, goal):
occ_grid_map = rospy.wait_for_message('/map', nav_msgs.OccupancyGrid)
goal.input_map = sensor_msgs.Image()
goal.input_map.header = occ_grid_map.header
goal.input_map.height = occ_grid_map.info.height
goal.input_map.width = occ_grid_map.info.width
goal.input_map.step = occ_grid_map.info.width
goal.input_map.encoding = 'mono8'
# Convert map into a black and white 8bit single-channel image (format 8UC1), which is 0 (black)
# for obstacles and unknown space, and 255 (white) for free space
for i in range(occ_grid_map.info.height):
for j in range(occ_grid_map.info.width):
pixel_value = b'\x00' if occ_grid_map.data[
i * occ_grid_map.info.width + j] in [-1, 100] else b'\xFF'
goal.input_map.data += pixel_value
# [/exploration/room_sequence_planning_server INFO 1592274444.350931252, 465.900000000]: ********Sequence planning started************
# terminate called after throwing an instance of 'cv_bridge::Exception'
# what(): [32SC1] is not a color format. but [mono8] is. The conversion does not make sense
# la img tie mal format pa planning y parece lento
# y fix [exploration/room_segmentation_server-17] escalating to SIGTERM
# [move_base-10] escalating to SIGTERM
rospy.Publisher('/exploration/img',
sensor_msgs.Image,
queue_size=1,
latch=True).publish(goal.input_map)
# ALTERNATIVE: read image from file
# import cv2
# from cv_bridge import CvBridge
# bridge = CvBridge()
# cv_image = cv2.imread('/home/jorge/catkin_ws/thorp/src/thorp/thorp_simulation/worlds/maps/cat_house.png',
# cv2.IMREAD_UNCHANGED)
# for i in range(cv_image.shape[0]):
# for j in range(cv_image.shape[1]):
# if cv_image[i][j] < 250:
# cv_image[i][j] = 0
# else:
# cv_image[i][j] = 255
# goal.input_map = bridge.cv2_to_imgmsg(cv_image, encoding='mono8')
rospy.Publisher('/exploration/img',
sensor_msgs.Image,
queue_size=1,
latch=True).publish(goal.input_map)
goal.map_origin = occ_grid_map.info.origin
goal.map_resolution = occ_grid_map.info.resolution
goal.return_format_in_meter = True
goal.return_format_in_pixel = True
goal.robot_radius = rospy.get_param(
'/move_base_flex/global_costmap/robot_radius', 0.18)
# those values are also needed by PlanRoomSequence and PlanRoomExploration, so share them as output keys
# the segmented map comes with its own origin and resolution, but are the same as for the input map
ud['map_image'] = goal.input_map
ud['map_origin'] = goal.map_origin
ud['map_resolution'] = goal.map_resolution
ud['robot_radius'] = goal.robot_radius
def __init__(
self,
param={
"low_H": 110,
"high_H": 120,
"low_S": 0,
"high_S": 254,
"low_V": 220,
"high_V": 230,
},
):
self.DIGITS_LOOKUP = {
(0, 0, 1, 1, 0, 1, 0): -1,
(0, 0, 0, 1, 0, 0, 0): "-",
(1, 1, 1, 0, 1, 1, 1): 0,
(0, 0, 1, 0, 0, 1, 0): 1,
(1, 0, 1, 1, 1, 0, 1): 2,
(1, 0, 1, 1, 0, 1, 1): 3,
(0, 1, 1, 1, 0, 1, 0): 4,
(1, 1, 0, 1, 0, 1, 1): 5,
(1, 1, 0, 1, 1, 1, 1): 6,
(1, 0, 1, 0, 0, 1, 0): 7,
(1, 1, 1, 1, 1, 1, 1): 8,
(1, 1, 1, 1, 0, 1, 1): 9,
}
self.heading_data = mrs_msgs.Float64Stamped()
self.nav = Position_Info()
self.camera_setting = sensor.CameraInfo()
self.camera_frame = sensor.Image()
self.sub_Camera = rospy.Subscriber(
"/uav1/bluefox_optflow/camera_info",
sensor.CameraInfo,
self.setting_callback,
)
self.sub = rospy.Subscriber("/uav1/bluefox_optflow/image_raw",
sensor.Image, self.camera_callback)
self.sub_hdg = self.sub = rospy.Subscriber(
"/uav1/control_manager/heading",
mrs_msgs.Float64Stamped,
self.heading_callback,
)
while not self.camera_frame.data:
pass
while self.camera_setting.K[0] <= 0:
pass
self.fx = self.camera_setting.K[0]
self.fy = self.camera_setting.K[5]
self.height = self.camera_setting.height
self.width = self.camera_setting.width
self.hfov = 2 * np.arctan2(self.width, 2 * self.fx)
self.vfov = 2 * np.arctan2(self.height, 2 * self.fy)
self.parameters = param
self.kernel = np.ones((5, 5), np.uint8)
self.bridge = CvBridge()
self.img = ""
def make_goal(self, ud, goal):
# Listen for current map
occ_grid_map = rospy.wait_for_message('/map', nav_msgs.OccupancyGrid)
goal.input_map = sensor_msgs.Image()
goal.input_map.header = occ_grid_map.header
goal.input_map.height = occ_grid_map.info.height
goal.input_map.width = occ_grid_map.info.width
goal.input_map.step = occ_grid_map.info.width
goal.input_map.encoding = 'mono8'
# Convert map into a black and white 8bit single-channel image (format 8UC1), which is 0 (black)
# for obstacles and unknown space, and 255 (white) for free space
for i in range(occ_grid_map.info.height):
for j in range(occ_grid_map.info.width):
pixel_value = b'\x00' if occ_grid_map.data[
i * occ_grid_map.info.width + j] in [-1, 100] else b'\xFF'
goal.input_map.data += pixel_value
# ALTERNATIVE: read image from file
# import cv2
# from cv_bridge import CvBridge
# bridge = CvBridge()
# cv_image = cv2.imread('/home/jorge/catkin_ws/thorp/src/thorp/thorp_simulation/worlds/maps/fun_house.png',
# cv2.IMREAD_UNCHANGED)
# for i in range(cv_image.shape[0]):
# for j in range(cv_image.shape[1]):
# if cv_image[i][j] < 250:
# cv_image[i][j] = 0
# else:
# cv_image[i][j] = 255
# goal.input_map = bridge.cv2_to_imgmsg(cv_image, encoding='mono8')
rospy.Publisher('/exploration/img',
sensor_msgs.Image,
queue_size=1,
latch=True).publish(goal.input_map)
goal.map_origin = occ_grid_map.info.origin
goal.map_resolution = occ_grid_map.info.resolution
goal.return_format_in_meter = True
goal.return_format_in_pixel = True
goal.robot_radius = rospy.get_param(
'/move_base_flex/global_costmap/robot_radius', 0.18)
# those values are also needed by PlanRoomSequence and PlanRoomExploration, so share them as output keys
# the segmented map comes with its own origin and resolution, but are the same as for the input map
ud['map_image'] = goal.input_map
ud['map_origin'] = goal.map_origin
ud['map_resolution'] = goal.map_resolution
ud['robot_radius'] = goal.robot_radius
def __init__(self, engine):
super().__init__('IProc_TestDisplayNode')
self.__window_name = "ROS2 PoseNet"
profile = qos.QoSProfile()
self.sub = self.create_subscription(msg.Image,
'/image',
self.msg_callback,
qos_profile=profile)
self.pub = self.create_publisher(msg.Image, '/recognized_image')
self.engine = engine
self.fps = ""
self.detectfps = ""
self.framecount = 0
self.detectframecount = 0
self.time1 = 0
self.time2 = 0
self.t1 = time.perf_counter()
self.t2 = time.perf_counter()
self.msg = msg.Image()
self.cv_bridge = CvBridge()
def result_cb(self, ud, status, result):
# make a random color rooms map to show in RViz
rooms_color_map = sensor_msgs.Image()
rooms_color_map.header = result.segmented_map.header
rooms_color_map.width = result.segmented_map.width
rooms_color_map.height = result.segmented_map.height
rooms_color_map.step = rooms_color_map.width * 3
rooms_color_map.encoding = 'rgb8'
for i in range(result.segmented_map.height):
for j in range(result.segmented_map.width):
idx = i * result.segmented_map.width * 4 + j * 4
val = result.segmented_map.data[idx:idx + 4]
room = struct.unpack('<BBBB', val)[0]
if room > 0:
random.seed(room)
r, g, b = random.randint(0, 255), random.randint(
0, 255), random.randint(0, 255)
rooms_color_map.data += struct.pack('<BBB', r, g, b)
else:
rooms_color_map.data += struct.pack('<BBB', 0, 0, 0)
self.img_pub.publish(rooms_color_map)