-for autonsystem and serial node (#78)

-use ros logging functions instead of print

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -135,22 +135,21 @@ def init_check(self):
         # (from both buggies if relevant)
         # covariance is less than 1 meter
         if (self.self_odom_msg == None):
-            print("WARNING: no available position estimate")
+            rospy.logwarn("WARNING: no available position estimate")
             return False
 
         if (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2):
-            print("checking position estimate certainty")
+            rospy.logwarn("checking position estimate certainty")
             return False
 
         # waits until covariance is acceptable to check heading
         with self.lock:
             self_pose = self.get_world_pose(self.self_odom_msg)
             current_heading = self_pose.theta
             closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y))
-        print("current heading: ", np.rad2deg(current_heading))
+        rospy.loginfo("current heading: ", np.rad2deg(current_heading))
         self.heading_publisher.publish(Float32(np.rad2deg(current_heading)))
 
-
         # headings are originally between -pi and pi
         # if they are negative, convert them to be between 0 and pi
         if current_heading < 0:
@@ -160,19 +159,19 @@ def init_check(self):
             closest_heading = 2*np.pi + closest_heading
 
         if (abs(current_heading - closest_heading) >= np.pi/2):
-            print("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: ", np.rad2deg(self_pose.theta))
+            rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: ", np.rad2deg(self_pose.theta))
             return False
 
         return True
 
     def tick_caller(self):
 
 
-        print("start checking initialization status")
+        rospy.loginfo("start checking initialization status")
         while ((not rospy.is_shutdown()) and not self.init_check()):
             self.init_check_publisher.publish(False)
             rospy.sleep(0.001)
-        print("done checking initialization status")
+        rospy.loginfo("done checking initialization status")
         self.init_check_publisher.publish(True)
         # initialize global trajectory index
 
@@ -308,9 +307,9 @@ def planner_tick(self):
     profile = args.profile
     left_curb_file = args.left_curb
 
-    print("\n\nStarting Controller: " + str(ctrl) + "\n\n")
-    print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
-    print("\n\nStarting at distance: " + str(start_dist) + "\n\n")
+    rospy.loginfo("\n\nStarting Controller: " + str(ctrl) + "\n\n")
+    rospy.loginfo("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
+    rospy.loginfo("\n\nStarting at distance: " + str(start_dist) + "\n\n")
 
     trajectory = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj)
     left_curb = None

rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py

@@ -61,7 +61,7 @@ def set_steering(self, msg):
             self.fresh_steer = True
 
     def writer_thread(self):
-        print('Starting sending alarm and steering to teensy!')
+        rospy.loginfo('Starting sending alarm and steering to teensy!')
         while True:
             if self.fresh_steer:
                 with self.lock:
@@ -74,12 +74,12 @@ def writer_thread(self):
             self.steer_send_rate.sleep()
 
     def reader_thread(self):
-        print('Starting reading odom from teensy!')
+        rospy.loginfo('Starting reading odom from teensy!')
         while True:
             packet = self.comms.read_packet()
             # print("trying to read odom")
             if isinstance(packet, Odometry):
-                # print("packet", packet.radio_seqnum, packet.gps_seqnum)
+                rospy.logdebug("packet", packet)
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
                 # convert to long lat