def __init__(self): """ """ self.tf_bridge = TfBridge() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.robot_camera = None self.camera_info = None self.camera_frame_id = None self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.base_frame_id = rospy.get_param("~base_frame_id", "odom") self.use_ar_tags = rospy.get_param("use_ar_tags", True) self.ar_tags_topic = rospy.get_param("ar_tags_topic", "ar_tracks") if self.use_ar_tags is True: self.ar_tags_tracks = [] self.ar_tags_sub = rospy.Subscriber(self.ar_tags_topic, WorldStamped, self.ar_tags_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_motion_capture = rospy.get_param("use_motion_capture", True) self.motion_capture_topic = rospy.get_param("motion_capture_topic", "motion_capture_tracks") if self.use_motion_capture is True: self.motion_capture_tracks = [] self.motion_capture_sub = rospy.Subscriber(self.motion_capture_topic, WorldStamped, self.motion_capture_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_object_perception = rospy.get_param("use_object_perception", True) self.object_perception_topic = rospy.get_param("object_perception_topic", "object_tracks") if self.use_object_perception is True: self.object_tracks = [] self.object_sub = rospy.Subscriber(self.object_perception_topic, WorldStamped, self.object_perception_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_human_perception = rospy.get_param("use_human_perception", True) self.human_perception_topic = rospy.get_param("human_perception_topic", "human_tracks") if self.use_human_perception is True: self.human_tracks = [] self.human_sub = rospy.Subscriber(self.human_perception_topic, WorldStamped, self.human_perception_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.publish_tf = rospy.get_param("~publish_tf", False) self.publish_viz = rospy.get_param("~publish_viz", True) self.publish_markers = rospy.get_param("~publish_markers", True) self.world_publisher = WorldPublisher("corrected_tracks") self.other_world_publisher = WorldPublisher("other_view_tracks") self.marker_publisher = MarkerPublisher("corrected_markers") self.robot_camera_clipnear = rospy.get_param("~robot_camera_clipnear", 0.1) self.robot_camera_clipfar = rospy.get_param("~robot_camera_clipfar", 25.0) use_simulation_gui = rospy.get_param("~use_simulation_gui", True) simulation_config_filename = rospy.get_param("~simulation_config_filename", "") cad_models_additional_search_path = rospy.get_param("~cad_models_additional_search_path", "") static_entities_config_filename = rospy.get_param("~static_entities_config_filename", "") robot_urdf_file_path = rospy.get_param("~robot_urdf_file_path", "") self.internal_simulator = InternalSimulator(use_simulation_gui, simulation_config_filename, cad_models_additional_search_path, static_entities_config_filename, robot_urdf_file_path, self.global_frame_id, self.base_frame_id) self.other_view_publisher = ViewPublisher("other_view") self.robot_perspective_monitor = RobotPerspectiveMonitor(self.internal_simulator) self.use_physical_monitoring = rospy.get_param("use_physical_monitoring", True) if self.use_physical_monitoring is True: self.physics_monitor = PhysicsMonitor(self.internal_simulator) self.use_perspective_monitoring = rospy.get_param("use_perspective_monitoring", True) if self.use_perspective_monitoring is True: self.perspective_monitor = HumanPerspectiveMonitor(self.internal_simulator) rospy.Service("/uwds3/get_perspective", GetPerspective, self.handle_perspective_taking) self.perspective_facts = [] self.egocentric_facts = [] self.physics_facts = [] self.rgb_camera_info_topic = rospy.get_param("~rgb_camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo("[internal_simulator] Subscribing to '/{}' topic...".format(self.rgb_camera_info_topic)) self.camera_info_subscriber = rospy.Subscriber(self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback)
class InternalSimulatorNode(object): """ Standalone node for the internal simulator """ def __init__(self): """ """ self.tf_bridge = TfBridge() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.robot_camera = None self.camera_info = None self.camera_frame_id = None self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.base_frame_id = rospy.get_param("~base_frame_id", "odom") self.use_ar_tags = rospy.get_param("use_ar_tags", True) self.ar_tags_topic = rospy.get_param("ar_tags_topic", "ar_tracks") if self.use_ar_tags is True: self.ar_tags_tracks = [] self.ar_tags_sub = rospy.Subscriber(self.ar_tags_topic, WorldStamped, self.ar_tags_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_motion_capture = rospy.get_param("use_motion_capture", True) self.motion_capture_topic = rospy.get_param("motion_capture_topic", "motion_capture_tracks") if self.use_motion_capture is True: self.motion_capture_tracks = [] self.motion_capture_sub = rospy.Subscriber(self.motion_capture_topic, WorldStamped, self.motion_capture_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_object_perception = rospy.get_param("use_object_perception", True) self.object_perception_topic = rospy.get_param("object_perception_topic", "object_tracks") if self.use_object_perception is True: self.object_tracks = [] self.object_sub = rospy.Subscriber(self.object_perception_topic, WorldStamped, self.object_perception_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.use_human_perception = rospy.get_param("use_human_perception", True) self.human_perception_topic = rospy.get_param("human_perception_topic", "human_tracks") if self.use_human_perception is True: self.human_tracks = [] self.human_sub = rospy.Subscriber(self.human_perception_topic, WorldStamped, self.human_perception_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) self.publish_tf = rospy.get_param("~publish_tf", False) self.publish_viz = rospy.get_param("~publish_viz", True) self.publish_markers = rospy.get_param("~publish_markers", True) self.world_publisher = WorldPublisher("corrected_tracks") self.other_world_publisher = WorldPublisher("other_view_tracks") self.marker_publisher = MarkerPublisher("corrected_markers") self.robot_camera_clipnear = rospy.get_param("~robot_camera_clipnear", 0.1) self.robot_camera_clipfar = rospy.get_param("~robot_camera_clipfar", 25.0) use_simulation_gui = rospy.get_param("~use_simulation_gui", True) simulation_config_filename = rospy.get_param("~simulation_config_filename", "") cad_models_additional_search_path = rospy.get_param("~cad_models_additional_search_path", "") static_entities_config_filename = rospy.get_param("~static_entities_config_filename", "") robot_urdf_file_path = rospy.get_param("~robot_urdf_file_path", "") self.internal_simulator = InternalSimulator(use_simulation_gui, simulation_config_filename, cad_models_additional_search_path, static_entities_config_filename, robot_urdf_file_path, self.global_frame_id, self.base_frame_id) self.other_view_publisher = ViewPublisher("other_view") self.robot_perspective_monitor = RobotPerspectiveMonitor(self.internal_simulator) self.use_physical_monitoring = rospy.get_param("use_physical_monitoring", True) if self.use_physical_monitoring is True: self.physics_monitor = PhysicsMonitor(self.internal_simulator) self.use_perspective_monitoring = rospy.get_param("use_perspective_monitoring", True) if self.use_perspective_monitoring is True: self.perspective_monitor = HumanPerspectiveMonitor(self.internal_simulator) rospy.Service("/uwds3/get_perspective", GetPerspective, self.handle_perspective_taking) self.perspective_facts = [] self.egocentric_facts = [] self.physics_facts = [] self.rgb_camera_info_topic = rospy.get_param("~rgb_camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo("[internal_simulator] Subscribing to '/{}' topic...".format(self.rgb_camera_info_topic)) self.camera_info_subscriber = rospy.Subscriber(self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback) def handle_perspective_taking(self, req): camera = HumanCamera() view_pose = Vector6D().from_msg(req.point_of_view.pose) egocentric_relations = [] if req.use_target is True: target_point = Vector3D().from_msg(req.target.point) _, _, _, visible_nodes = self.internal_simulator.get_camera_view(view_pose, camera, target=target_point) else: _, _, _, visible_nodes = self.internal_simulator.get_camera_view(view_pose, camera) for node1 in visible_nodes: for node2 in visible_nodes: if node1 != node2: bbox1 = node1.bbox bbox2 = node2.bbox if is_right_of(bbox1, bbox2) is True: description = node1.description+"("+node1.id[:6]+") is right of "+node2.description+"("+node2.id[:6]+")" egocentric_relations.append(Fact(node1.id, description, predicate="right_of", object=node2.id)) if is_left_of(bbox1, bbox2) is True: description = node1.description+"("+node1.id[:6]+") is left of "+node2.description+"("+node2.id[:6]+")" egocentric_relations.append(Fact(node1.id, description, predicate="left_of", object=node2.id)) if is_behind(bbox1, bbox2) is True: description = node1.description+"("+node1.id[:6]+") is behind "+node2.description+"("+node2.id[:6]+")" egocentric_relations.append(Fact(node1.id, description, predicate="behind", object=node2.id)) return visible_nodes, egocentric_relations, True, "" def object_perception_callback(self, world_msg): object_tracks = [] for node in world_msg.world.scene: object_tracks.append(SceneNode().from_msg(node)) self.object_tracks = object_tracks def human_perception_callback(self, world_msg): human_tracks = [] for node in world_msg.world.scene: human_tracks.append(SceneNode().from_msg(node)) self.human_tracks = human_tracks def ar_tags_callback(self, world_msg): ar_tags_tracks = [] for node in world_msg.world.scene: ar_tags_tracks.append(SceneNode().from_msg(node)) self.ar_tags_tracks = ar_tags_tracks def motion_capture_callback(self, world_msg): motion_capture_tracks = [] for node in world_msg.world.scene: motion_capture_tracks.append(SceneNode().from_msg(node)) self.motion_capture_tracks = motion_capture_tracks 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 def run(self): while not rospy.is_shutdown(): rospy.spin()
class ObjectPerceptionNode(object): def __init__(self): """ """ self.tf_bridge = TfBridge() self.cv_bridge = CvBridge() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.global_frame_id = rospy.get_param("~global_frame_id", "odom") object_detector_weights_filename = rospy.get_param( "~object_detector_weights_filename", "") object_detector_model_filename = rospy.get_param( "~object_detector_model_filename", "") object_detector_config_filename = rospy.get_param( "~object_detector_config_filename", "") enable_cuda = rospy.get_param("~enable_cuda", True) use_mask_rcnn = rospy.get_param("~use_mask_rcnn", False) if use_mask_rcnn is True: self.object_detector = MaskRCNNDetector( object_detector_weights_filename, object_detector_model_filename, object_detector_config_filename, enable_cuda=enable_cuda) else: self.object_detector = SSDDetector( object_detector_weights_filename, object_detector_model_filename, object_detector_config_filename, enable_cuda=enable_cuda) self.n_init = rospy.get_param("~n_init", 1) self.max_iou_distance = rospy.get_param("~max_iou_distance", 0.98) self.max_appearance_distance = rospy.get_param( "~max_appearance_distance", 0.25) self.max_lost = rospy.get_param("~max_lost", 4) self.max_age = rospy.get_param("~max_age", 12) self.robot_camera = None self.camera_info = None self.table = None self.events = [] self.robot_camera_clipnear = rospy.get_param("~robot_camera_clipnear", 0.1) self.robot_camera_clipfar = rospy.get_param("~robot_camera_clipfar", 25.0) self.object_tracker = MultiObjectTracker(iou_cost, appearance_cost, self.max_iou_distance, self.max_appearance_distance, self.n_init, self.max_lost, self.max_age, use_tracker=True) appearance_features_weights_filename = rospy.get_param( "~appearance_features_weights_filename", "") appearance_features_model_filename = rospy.get_param( "~appearance_features_model_filename", "") self.appearance_features_estimator = AppearanceFeaturesEstimator( appearance_features_weights_filename, appearance_features_model_filename) self.shape_estimator = ShapeEstimator() self.color_features_estimator = ColorFeaturesEstimator() self.object_pose_estimator = ObjectPoseEstimator() self.publish_tf = rospy.get_param("~publish_tf", True) self.publish_viz = rospy.get_param("~publish_viz", True) self.publish_markers = rospy.get_param("~publish_markers", True) self.world_publisher = WorldPublisher("object_tracks") self.view_publisher = ViewPublisher("object_perception") self.marker_publisher = MarkerPublisher("object_markers") self.use_depth = rospy.get_param("~use_depth", False) self.rgb_image_topic = rospy.get_param("~rgb_image_topic", "/camera/rgb/image_raw") self.rgb_camera_info_topic = rospy.get_param( "~rgb_camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_camera_info_topic)) self.camera_info_subscriber = rospy.Subscriber( self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback) if self.use_depth is True: self.depth_image_topic = rospy.get_param( "~depth_image_topic", "/camera/depth/image_raw") self.depth_camera_info_topic = rospy.get_param( "~depth_camera_info_topic", "/camera/depth/camera_info") rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = message_filters.Subscriber( self.rgb_image_topic, Image) rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.depth_image_topic)) self.depth_image_sub = message_filters.Subscriber( self.depth_image_topic, Image) self.sync = message_filters.ApproximateTimeSynchronizer( [self.rgb_image_sub, self.depth_image_sub], DEFAULT_SENSOR_QUEUE_SIZE, 0.1, allow_headerless=True) self.sync.registerCallback(self.observation_callback) else: rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = rospy.Subscriber( self.rgb_image_topic, Image, self.observation_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE) 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 observation_callback(self, bgr_image_msg, depth_image_msg=None): """ """ if self.robot_camera is not None: header = bgr_image_msg.header header.frame_id = self.global_frame_id bgr_image = self.cv_bridge.imgmsg_to_cv2(bgr_image_msg, "bgr8") rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) if depth_image_msg is not None: depth_image = self.cv_bridge.imgmsg_to_cv2(depth_image_msg) else: depth_image = None _, self.image_height, self.image_width = bgr_image.shape 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( "[object_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: self.frame_count %= self.n_frame all_nodes, events = self.perception_pipeline( view_pose, rgb_image, depth_image=depth_image, time=header.stamp) self.world_publisher.publish(all_nodes, events, header) if self.publish_markers is True: self.marker_publisher.publish(all_nodes, header) if self.publish_tf is True: self.tf_bridge.publish_tf_frames(all_nodes, events, header) self.frame_count += 1 def perception_pipeline(self, view_pose, rgb_image, depth_image=None, time=None): ###################################################### # Detection ###################################################### pipeline_timer = cv2.getTickCount() detection_timer = cv2.getTickCount() detections = [] if self.frame_count == 0: detections = self.object_detector.detect(rgb_image, depth_image=depth_image) else: detections = [] detection_fps = cv2.getTickFrequency() / (cv2.getTickCount() - detection_timer) #################################################################### # Features estimation #################################################################### features_timer = cv2.getTickCount() if self.frame_count == 0: self.appearance_features_estimator.estimate(rgb_image, detections) self.color_features_estimator.estimate(rgb_image, detections) features_fps = cv2.getTickFrequency() / (cv2.getTickCount() - features_timer) ###################################################### # Tracking ###################################################### tracking_timer = cv2.getTickCount() if self.frame_count == 0: self.object_tracks = self.object_tracker.update( rgb_image, detections, depth_image=depth_image, time=time) else: self.object_tracks = self.object_tracker.update( rgb_image, [], depth_image=depth_image, time=time) tracks = self.object_tracks tracking_fps = cv2.getTickFrequency() / (cv2.getTickCount() - tracking_timer) ######################################################## # Pose & Shape estimation ######################################################## pose_timer = cv2.getTickCount() self.object_pose_estimator.estimate(tracks, view_pose, self.robot_camera) self.shape_estimator.estimate(rgb_image, tracks, self.robot_camera) pose_fps = cv2.getTickFrequency() / (cv2.getTickCount() - pose_timer) pipeline_fps = cv2.getTickFrequency() / (cv2.getTickCount() - pipeline_timer) ######################################################## # Visualization ######################################################## if self.publish_viz is True: self.view_publisher.publish(rgb_image, tracks, time, overlay_image=None, fps=pipeline_fps, view_pose=view_pose, camera=self.robot_camera) all_nodes = tracks return all_nodes, self.events def run(self): while not rospy.is_shutdown(): rospy.spin()
def __init__(self): """ """ self.tf_bridge = TfBridge() self.cv_bridge = CvBridge() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.global_frame_id = rospy.get_param("~global_frame_id", "odom") object_detector_weights_filename = rospy.get_param( "~object_detector_weights_filename", "") object_detector_model_filename = rospy.get_param( "~object_detector_model_filename", "") object_detector_config_filename = rospy.get_param( "~object_detector_config_filename", "") enable_cuda = rospy.get_param("~enable_cuda", True) use_mask_rcnn = rospy.get_param("~use_mask_rcnn", False) if use_mask_rcnn is True: self.object_detector = MaskRCNNDetector( object_detector_weights_filename, object_detector_model_filename, object_detector_config_filename, enable_cuda=enable_cuda) else: self.object_detector = SSDDetector( object_detector_weights_filename, object_detector_model_filename, object_detector_config_filename, enable_cuda=enable_cuda) self.n_init = rospy.get_param("~n_init", 1) self.max_iou_distance = rospy.get_param("~max_iou_distance", 0.98) self.max_appearance_distance = rospy.get_param( "~max_appearance_distance", 0.25) self.max_lost = rospy.get_param("~max_lost", 4) self.max_age = rospy.get_param("~max_age", 12) self.robot_camera = None self.camera_info = None self.table = None self.events = [] self.robot_camera_clipnear = rospy.get_param("~robot_camera_clipnear", 0.1) self.robot_camera_clipfar = rospy.get_param("~robot_camera_clipfar", 25.0) self.object_tracker = MultiObjectTracker(iou_cost, appearance_cost, self.max_iou_distance, self.max_appearance_distance, self.n_init, self.max_lost, self.max_age, use_tracker=True) appearance_features_weights_filename = rospy.get_param( "~appearance_features_weights_filename", "") appearance_features_model_filename = rospy.get_param( "~appearance_features_model_filename", "") self.appearance_features_estimator = AppearanceFeaturesEstimator( appearance_features_weights_filename, appearance_features_model_filename) self.shape_estimator = ShapeEstimator() self.color_features_estimator = ColorFeaturesEstimator() self.object_pose_estimator = ObjectPoseEstimator() self.publish_tf = rospy.get_param("~publish_tf", True) self.publish_viz = rospy.get_param("~publish_viz", True) self.publish_markers = rospy.get_param("~publish_markers", True) self.world_publisher = WorldPublisher("object_tracks") self.view_publisher = ViewPublisher("object_perception") self.marker_publisher = MarkerPublisher("object_markers") self.use_depth = rospy.get_param("~use_depth", False) self.rgb_image_topic = rospy.get_param("~rgb_image_topic", "/camera/rgb/image_raw") self.rgb_camera_info_topic = rospy.get_param( "~rgb_camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_camera_info_topic)) self.camera_info_subscriber = rospy.Subscriber( self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback) if self.use_depth is True: self.depth_image_topic = rospy.get_param( "~depth_image_topic", "/camera/depth/image_raw") self.depth_camera_info_topic = rospy.get_param( "~depth_camera_info_topic", "/camera/depth/camera_info") rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = message_filters.Subscriber( self.rgb_image_topic, Image) rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.depth_image_topic)) self.depth_image_sub = message_filters.Subscriber( self.depth_image_topic, Image) self.sync = message_filters.ApproximateTimeSynchronizer( [self.rgb_image_sub, self.depth_image_sub], DEFAULT_SENSOR_QUEUE_SIZE, 0.1, allow_headerless=True) self.sync.registerCallback(self.observation_callback) else: rospy.loginfo( "[object_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = rospy.Subscriber( self.rgb_image_topic, Image, self.observation_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE)
def __init__(self): """ """ self.tf_bridge = TfBridge() self.n_frame = rospy.get_param("~n_frame", 4) self.frame_count = 0 self.global_frame_id = rospy.get_param("~global_frame_id", "odom") self.color = rospy.get_param("~color", "red") self.debug_topics = rospy.get_param("~debug_topics", True) self.color_detector = ColorDetector(debug_topics=self.debug_topics, color=self.color) self.n_init = rospy.get_param("~n_init", 1) self.max_iou_distance = rospy.get_param("~max_iou_distance", 0.98) self.max_lost = rospy.get_param("~max_lost", 4) self.max_age = rospy.get_param("~max_age", 300) self.bridge = CvBridge() self.robot_camera = None self.camera_info = None self.table = None self.events = [] self.robot_camera_clipnear = rospy.get_param("~robot_camera_clipnear", 0.1) self.robot_camera_clipfar = rospy.get_param("~robot_camera_clipfar", 25.0) self.object_tracker = MultiObjectTracker(iou_cost, centroid_cost, self.max_iou_distance, None, self.n_init, self.max_lost, self.max_age, use_tracker=True) self.shape_estimator = ShapeEstimator() self.object_pose_estimator = ObjectPoseEstimator() self.publish_tf = rospy.get_param("~publish_tf", False) self.publish_viz = rospy.get_param("~publish_viz", True) self.world_publisher = WorldPublisher("color_object_tracks") self.view_publisher = ViewPublisher("color_object_perception") self.marker_publisher = MarkerPublisher("color_object_markers") self.use_depth = rospy.get_param("~use_depth", False) self.rgb_image_topic = rospy.get_param("~rgb_image_topic", "/camera/rgb/image_raw") self.rgb_camera_info_topic = rospy.get_param( "~rgb_camera_info_topic", "/camera/rgb/camera_info") rospy.loginfo( "[color_perception] Subscribing to '/{}' topic...".format( self.rgb_camera_info_topic)) self.camera_info_subscriber = rospy.Subscriber( self.rgb_camera_info_topic, CameraInfo, self.camera_info_callback) if self.use_depth is True: self.depth_image_topic = rospy.get_param( "~depth_image_topic", "/camera/depth/image_raw") self.depth_camera_info_topic = rospy.get_param( "~depth_camera_info_topic", "/camera/depth/camera_info") rospy.loginfo( "[color_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = message_filters.Subscriber( self.rgb_image_topic, Image) rospy.loginfo( "[color_perception] Subscribing to '/{}' topic...".format( self.depth_image_topic)) self.depth_image_sub = message_filters.Subscriber( self.depth_image_topic, Image) self.sync = message_filters.ApproximateTimeSynchronizer( [self.rgb_image_sub, self.depth_image_sub], DEFAULT_SENSOR_QUEUE_SIZE, 0.1, allow_headerless=True) self.sync.registerCallback(self.observation_callback) else: rospy.loginfo( "[color_perception] Subscribing to '/{}' topic...".format( self.rgb_image_topic)) self.rgb_image_sub = rospy.Subscriber( self.rgb_image_topic, Image, self.observation_callback, queue_size=DEFAULT_SENSOR_QUEUE_SIZE)