def __init__(self):
"""Default constructor"""
self.rgb_image_topic = rospy.get_param("~rgb_image_topic",
"/camera/rgb/image_raw")
self.camera_publisher = rospy.Publisher(self.rgb_image_topic,
sensor_msgs.msg.Image,
queue_size=1)
self.camera_pub_frequency = rospy.get_param("~camera_pub_frequency",
30)
self.camera_id = rospy.get_param("~camera_id", 0)
self.bridge = CvBridge()
self.camera_info_topic = rospy.get_param("~camera_info_topic",
"/camera/rgb/camera_info")
self.camera_info = sensor_msgs.msg.CameraInfo()
self.camera_info_publisher = rospy.Publisher(
self.camera_info_topic, sensor_msgs.msg.CameraInfo, queue_size=1)
self.camera_frame_id = rospy.get_param("~camera_frame_id",
"camera_link")
self.camera_info.header.frame_id = self.camera_frame_id
self.capture = cv2.VideoCapture(self.camera_id)
ok, frame = self.capture.read()
if frame is None:
rospy.logerr(
"Unable to read the camera {}! Shutdown the camera publisher.."
.format(self.camera_id))
else:
height, width, _ = frame.shape
self.camera = Camera(height=height, width=width)
self.camera_info = self.camera.to_msg().info
self.timer = rospy.Timer(
rospy.Duration(1.0 / self.camera_pub_frequency),
self.timer_callback)
rospy.loginfo("Camera publisher ready !")
while not rospy.is_shutdown():
rospy.spin()
self.capture.release()
def camera_info_callback(self, msg):
""" """
if self.camera_info is None:
rospy.loginfo("[perception] Camera info received !")
self.camera_info = msg
self.camera_frame_id = msg.header.frame_id
self.robot_camera = Camera().from_msg(msg,
clipnear=self.robot_camera_clipnear,
clipfar=self.robot_camera_clipfar)
def camera_info_callback(self, msg):
""" """
if self.camera_info is None:
rospy.loginfo("[perception] Camera info received !")
self.camera_info = msg
self.camera_frame_id = msg.header.frame_id
self.camera_matrix = np.array(msg.K).reshape((3, 3))
self.dist_coeffs = np.array(msg.D)
self.robot_camera = Camera().from_msg(msg,
fov=self.robot_camera_fov,
clipnear=self.robot_camera_clipnear,
clipfar=self.robot_camera_clipfar)
def camera_info_callback(self, msg):
""" """
if self.camera_info is None:
rospy.loginfo("[perception] Camera info received !")
self.camera_info = msg
self.camera_frame_id = msg.header.frame_id
self.robot_camera = Camera().from_msg(msg,
clipnear=self.robot_camera_clipnear,
clipfar=self.robot_camera_clipfar)
if self.internal_simulator.is_robot_loaded() is True:
success, view_pose = self.tf_bridge.get_pose_from_tf(self.global_frame_id, self.camera_frame_id)
if success is not True:
rospy.logwarn("[human_perception] The camera sensor is not localized in world space (frame '{}'), please check if the sensor frame is published in /tf".format(self.global_frame_id))
else:
header = msg.header
header.frame_id = self.global_frame_id
self.frame_count %= self.n_frame
object_tracks = self.ar_tags_tracks + self.object_tracks
person_tracks = [f for f in self.human_tracks if f.label == "person"]
corrected_object_tracks, self.physics_facts = self.physics_monitor.monitor(object_tracks, person_tracks, header.stamp)
if self.use_perspective_monitoring is True:
if self.frame_count == 3:
monitoring_timer = cv2.getTickCount()
perspective_facts = []
face_tracks = [f for f in self.human_tracks if f.label == "face"]
person_tracks = [f for f in self.human_tracks if f.label == "person"]
success, other_image, other_visible_tracks, perspective_facts = self.perspective_monitor.monitor(face_tracks, person_tracks, header.stamp)
monitoring_fps = cv2.getTickFrequency() / (cv2.getTickCount()-monitoring_timer)
if success:
self.perspective_facts = [s for s in perspective_facts if s.predicate == "visible_by"]
self.other_world_publisher.publish(other_visible_tracks, perspective_facts+self.physics_facts, header)
self.other_view_publisher.publish(other_image, other_visible_tracks, header.stamp, fps=monitoring_fps)
_, self.egocentric_facts = self.robot_perspective_monitor.monitor(object_tracks, person_tracks, self.robot_camera, view_pose, header.stamp)
corrected_tracks = self.internal_simulator.get_static_entities() + self.human_tracks + corrected_object_tracks
events = self.physics_facts + self.perspective_facts + self.egocentric_facts
self.world_publisher.publish(corrected_tracks, events, header)
if self.publish_tf is True:
self.tf_bridge.publish_tf_frames(corrected_tracks, action_events , header)
if self.publish_markers is True:
self.marker_publisher.publish(corrected_tracks, header)
self.frame_count += 1