From 1fe8f8b7a76ac4be51feec97f0c8209b29f3648e Mon Sep 17 00:00:00 2001 From: Jackack Date: Wed, 3 Apr 2024 21:11:05 -0400 Subject: [PATCH] -for autonsystem and serial node -use ros logging functions instead of print --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 22 ++++++++----------- .../buggy/scripts/serial/ros_to_bnyahaj.py | 6 ++--- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index b598bf9..11e6e5b 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -135,11 +135,11 @@ 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 @@ -147,10 +147,9 @@ def init_check(self): self_pose, _ = self.get_world_pose_and_speed(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,7 +159,7 @@ 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 @@ -168,17 +167,14 @@ def init_check(self): 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 - with self.lock: - _, _ = self.get_world_pose_and_speed(self.self_odom_msg) - t_planner = threading.Thread(target=self.planner_thread) t_controller = threading.Thread(target=self.local_controller_thread) @@ -314,9 +310,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 diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index b7a77c1..7f91a38 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -57,7 +57,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: @@ -70,12 +70,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