Merge pull request #45 from kahlering/devel

improved logging

config/boxy.yaml

@@ -1,5 +1,6 @@
 general_options:
   sample_period: 0.05 # time[s] between points of generated joint trajectory
+  debug: False # enables debug prints and prints the behavior tree state in the terminal
   map_frame: map
   fill_velocity_values: True # due to a bug, the pr2 does not want the velocity values to be filled
   joint_convergence_threshold: 0.01 # when the velocities fall below this value, the planning succeeds
@@ -36,7 +37,6 @@ plugins:
     wiggle_detection_threshold: 10
     min_wiggle_frequency: 3
 behavior_tree:
-  debug: False # prints the behavior tree state in the terminal
   tree_tick_rate: 0.1 # how often the tree updates. lower numbers increase responsiveness, but waste cpu time while idle
 collision_avoidance:
   distance_thresholds:

config/donbot.yaml

@@ -1,5 +1,6 @@
 general_options:
   sample_period: 0.05 # time[s] between points of generated joint trajectory
+  debug: False # enables debug prints and prints the behavior tree state in the terminal
   map_frame: map
   fill_velocity_values: True # due to a bug, the pr2 does not want the velocity values to be filled
   joint_convergence_threshold: 0.01 # when the velocities fall below this value, the planning succeeds
@@ -35,7 +36,6 @@ plugins:
     wiggle_detection_threshold: 10
     min_wiggle_frequency: 3
 behavior_tree:
-  debug: False # prints the behavior tree state in the terminal
   tree_tick_rate: 0.1 # how often the tree updates. lower numbers increase responsiveness, but waste cpu time while idle
 collision_avoidance:
   distance_thresholds:

config/hsr.yaml

@@ -1,5 +1,6 @@
 general_options:
   sample_period: 0.05 # time[s] between points of generated joint trajectory
+  debug: False # 
   map_frame: map
   fill_velocity_values: True # due to a bug, the pr2 does not want the velocity values to be filled
   joint_convergence_threshold: 0.01 # when the velocities fall below this value, the planning succeeds
@@ -32,7 +33,6 @@ plugins:
     wiggle_detection_threshold: 10
     min_wiggle_frequency: 3
 behavior_tree:
-  debug: False # prints the behavior tree state in the terminal
   tree_tick_rate: 0.1 # how often the tree updates. lower numbers increase responsiveness, but waste cpu time while idle
 collision_avoidance:
   distance_thresholds:

config/pr2.yaml

@@ -1,6 +1,7 @@
 general_options:
   sample_period: 0.05 # time[s] between points of generated joint trajectory
   map_frame: map
+  debug: False # enables debug prints and prints the behavior tree state in the terminal
   fill_velocity_values: True # due to a bug, the pr2 does not want the velocity values to be filled
   joint_convergence_threshold: 0.01 # when the velocities fall below this value, the planning succeeds
   joint_vel_limit: # giskard will use the min of this number and limits from the urdf
@@ -36,7 +37,6 @@ plugins:
     wiggle_detection_threshold: 10
     min_wiggle_frequency: 3
 behavior_tree:
-  debug: False # prints the behavior tree state in the terminal
   tree_tick_rate: 0.1 # how often the tree updates. lower numbers increase responsiveness, but waste cpu time while idle
 collision_avoidance:
   distance_thresholds:

scripts/joint_trajectory_splitter.py

@@ -42,12 +42,12 @@ def __init__(self):
         self.client_topics = rospy.get_param('~client_topics', [])
         self.number_of_clients = len(self.state_topics)
         if self.number_of_clients != len(self.client_topics):
-            logging.logerr('Joint Trajector Splitter: number of state and action topics do not match')
+            logging.logerr('number of state and action topics do not match')
             exit()
 
 
         if self.number_of_clients == 0:
-            logging.logerr('Joint Trajector Splitter: the state_topic or client_topic parameter is empty')
+            logging.logerr('the state_topic or client_topic parameter is empty')
             exit()
 
         self.client_type = []
@@ -60,12 +60,12 @@ def __init__(self):
                     type = rostopic.get_info_text(self.client_topics[i] + '/goal').split('\n')[0][6:]
                     self.client_type.append(type)
                 except rostopic.ROSTopicException:
-                    logging.logerr('Joint Trajector Splitter: unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(self.client_topics[i]))
+                    logging.logerr('unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(self.client_topics[i]))
                     exit()
                 except rospy.ROSException as e:
                     if e.message == 'rospy shutdown':
                         exit()
-                    logging.loginfo('Joint Trajector Splitter: waiting for state topic {}'.format(self.state_topics[i]))
+                    logging.loginfo('waiting for state topic {}'.format(self.state_topics[i]))
 
         self.state_type = []
         for i in range(self.number_of_clients):
@@ -74,7 +74,7 @@ def __init__(self):
                 self.state_type.append(type)
             except rostopic.ROSTopicException:
                 logging.logerr(
-                    'Joint Trajector Splitter: unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(
+                    'unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(
                         self.state_topics[i]))
                 exit()
 
@@ -84,10 +84,10 @@ def __init__(self):
             elif self.client_type[i] == 'pr2_controllers_msgs/JointTrajectoryActionGoal':
                 self.action_clients.append(actionlib.SimpleActionClient(self.client_topics[i], pr2_controllers_msgs.msg.JointTrajectoryAction))
             else:
-                logging.logerr('Joint Trajector Splitter: wrong client topic type:' + self.client_type[i] + '\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal')
+                logging.logerr('wrong client topic type:' + self.client_type[i] + '\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal')
                 exit()
             self.joint_names.append(rospy.wait_for_message(self.state_topics[i], control_msgs.msg.JointTrajectoryControllerState).joint_names)
-            logging.loginfo('Joint Trajector Splitter: connected to {}'.format(self.client_topics[i]))
+            logging.loginfo('connected to {}'.format(self.client_topics[i]))
 
         self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState()
         total_number_joints = 0
@@ -112,7 +112,7 @@ def __init__(self):
             elif self.state_type[i] == 'pr2_controllers_msgs/JointTrajectoryControllerState':
                 rospy.Subscriber(self.state_topics[i], pr2_controllers_msgs.msg.JointTrajectoryControllerState, self.state_cb_update)
             else:
-                logging.logerr('Joint Trajector Splitter: wrong state topic type ' + self.state_type[i] + '\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState')
+                logging.logerr('wrong state topic type ' + self.state_type[i] + '\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState')
                 exit()
 
         rospy.Subscriber(self.state_topics[0], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_publish)
@@ -137,7 +137,7 @@ def callback(self, goal):
                 try:
                     index_list.append(goal.trajectory.joint_names.index(joint_name))
                 except ValueError:
-                    logging.logerr('Joint Trajector Splitter: the goal does not contain the joint ' + joint_name + ' but it is published by one of the state topics')
+                    logging.logerr('the goal does not contain the joint ' + joint_name + ' but it is published by one of the state topics')
                     result = control_msgs.msg.FollowJointTrajectoryResult()
                     result.error_code = control_msgs.msg.FollowJointTrajectoryResult.INVALID_GOAL
                     self._as.set_aborted(result)
@@ -191,7 +191,7 @@ def callback(self, goal):
             now = rospy.Time.now()
             timeout = timeout - (now - start)
             if not finished_before_timeout:
-                logging.logwarn("Joint Trajector Splitter: Client took to long to finish action")
+                logging.logwarn("Client took to long to finish action")
                 self.success = False
                 self._as.set_aborted()
                 break

src/giskardpy/garden.py

@@ -67,7 +67,7 @@ def initialize_god_map():
                                                        timeout=5.0).joint_names
         except ROSException as e:
             logging.logerr(u'state topic not available')
-            logging.logerr(e)
+            logging.logerr(str(e))
         else:
             break
         rospy.sleep(0.5)

src/giskardpy/identifier.py

@@ -38,11 +38,13 @@
 gui = rosparam + [u'enable_gui']
 data_folder = rosparam + [u'path_to_data_folder']
 
+
 # config file
 # general options
 general_options = rosparam + [u'general_options']
 sample_period = general_options + [u'sample_period']
 map_frame = general_options + [u'map_frame']
+debug = general_options + [u'debug']
 fill_velocity_values = general_options + [u'fill_velocity_values']
 joint_convergence_threshold = general_options + [u'joint_convergence_threshold']
 
@@ -76,7 +78,6 @@
 
 # behavior tree
 behavior_tree = rosparam + [u'behavior_tree']
-debug = behavior_tree + [u'debug']
 tree_tick_rate = behavior_tree + [u'tree_tick_rate']
 
 # collision avoidance

src/giskardpy/logging.py

@@ -1,17 +1,56 @@
 import rospy
+from inspect import currentframe, getframeinfo
+from giskardpy import identifier
+
+
+def debug():
+    if debug.param == None:
+        l = identifier.debug
+        param_name = '/giskard/' + '/'.join(s for s in l[1:])
+        debug.param = rospy.get_param(param_name)
+        return debug.param
+    else:
+        return debug.param
+
+debug.param = None
+
+
+def generate_debug_msg(msg):
+    node_name = rospy.get_name()
+    frameinfo = getframeinfo(currentframe().f_back.f_back)
+    file_info = frameinfo.filename.split('/')[-1] + ' line ' + str(frameinfo.lineno)
+    new_msg = ('\nnode: ' + node_name + '\n' +
+               'file: ' + file_info + '\n' +
+               'message: ' + msg + '\n')
+    return new_msg
+
+
+def generate_msg(msg):
+    if(debug()):
+        return generate_debug_msg(msg)
+    else:
+        node_name = rospy.get_name()
+        new_msg = '[' + node_name + ']' + ': ' + msg
+        return new_msg
+
 
 def logdebug(msg):
-    rospy.logdebug(msg)
+    final_msg = generate_debug_msg(msg)
+    rospy.logdebug(final_msg)
 
 def loginfo(msg):
-    rospy.loginfo(msg)
+    final_msg = generate_msg(msg)
+    rospy.loginfo(final_msg)
 
 def logwarn(msg):
-    rospy.logwarn(msg)
+    final_msg = generate_msg(msg)
+    rospy.logwarn(final_msg)
 
 def logerr(msg):
-    rospy.logerr(msg)
+    final_msg = generate_msg(msg)
+    rospy.logerr(final_msg)
 
 def logfatal(msg):
-    rospy.logfatal(msg)
+    final_msg = generate_msg(msg)
+    rospy.logfatal(final_msg)