Client: Setup logger for visual pose watchdog

Drone/visual_pose_watchdog.py

@@ -1,15 +1,34 @@
 import rospy
+import sys
 import time
+import logging
 from std_srvs.srv import Trigger
 from geometry_msgs.msg import PoseStamped
 
+logging.basicConfig(  # TODO all prints as logs
+   level=logging.DEBUG, # INFO
+   stream=sys.stdout,
+   format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
+   handlers=[
+       logging.StreamHandler(sys.stdout),
+   ])
+
+handler = logging.StreamHandler(sys.stdout)
+handler.setLevel(logging.DEBUG)
+formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s")
+handler.setFormatter(formatter)
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.DEBUG)
+logger.addHandler(handler)
+
 land = rospy.ServiceProxy('/land', Trigger)
 
 visual_pose_last_timestamp = 0
 visual_pose_timeout = 2.
 
 rospy.init_node('visual_pose_watchdog')
-rospy.loginfo('visual_pose_watchdog inited')
+logger.info('visual_pose_watchdog inited')
 
 def visual_pose_callback(data):
     global visual_pose_last_timestamp
@@ -18,7 +37,7 @@ def visual_pose_callback(data):
 def watchdog_callback(event):
     global visual_pose_last_timestamp
     if (time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_last_timestamp != 0:
-        rospy.loginfo('Visual pose data is too old, landing...')
+        logger.info('Visual pose data is too old, landing...')
         land()
 
 rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)