From 29867d8187ad47b3ce6143b46c2bcc2002830d78 Mon Sep 17 00:00:00 2001
From: Jackack <hjack2667@gmail.com>
Date: Fri, 5 Apr 2024 23:20:33 -0400
Subject: [PATCH] fix logging, set default controller to stanley

---
 rb_ws/src/buggy/launch/sc-main.launch            | 2 +-
 rb_ws/src/buggy/launch/sim_2d_2buggies.launch    | 8 ++++----
 rb_ws/src/buggy/scripts/auton/autonsystem.py     | 4 ++--
 rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py | 8 ++++----
 4 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/rb_ws/src/buggy/launch/sc-main.launch b/rb_ws/src/buggy/launch/sc-main.launch
index 96251ea5..88ab6b72 100755
--- a/rb_ws/src/buggy/launch/sc-main.launch
+++ b/rb_ws/src/buggy/launch/sc-main.launch
@@ -1,7 +1,7 @@
 <!-- roslaunch buggy main.launch -->
 
 <launch>
-    <arg name="controller"      default="mpc" />
+    <arg name="controller"      default="stanley" />
     <arg name="start_dist"      default="0.0" />
     <arg name="path"      default="buggycourse_safe_1.json" />
 
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
index 21ef78fd..27fe48cb 100755
--- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,11 +1,11 @@
 <launch>
-    <arg name="sc_controller"     default="mpc" />
+    <arg name="sc_controller"     default="stanley" />
     <arg name="sc_start_pos"    default="Hill1_SC" />
     <arg name="sc_velocity"         default="12.0" />
 
     <arg name="nand_controller"     default="stanley" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
-    <arg name="sc_path"           default="buggycourse_raceline.json" />
+    <arg name="sc_path"           default="buggycourse_safe_1.json" />
     <arg name="nand_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
 
@@ -35,11 +35,11 @@
         args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/>
 
     <!-- NAND is not aware of SC -->
-    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND" />
+    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND --left_curb curb.json" />
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
-    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND" />
+    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND --left_curb curb.json" />
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 58b9ef14..9b5d562e 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -147,7 +147,7 @@ def init_check(self):
             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))
-        rospy.loginfo("current heading: ", np.rad2deg(current_heading))
+        rospy.loginfo("current heading: " + str(np.rad2deg(current_heading)))
         self.heading_publisher.publish(Float32(np.rad2deg(current_heading)))
 
         # headings are originally between -pi and pi
@@ -159,7 +159,7 @@ def init_check(self):
             closest_heading = 2*np.pi + closest_heading
 
         if (abs(current_heading - closest_heading) >= np.pi/2):
-            rospy.logwarn("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]: " + str(np.rad2deg(self_pose.theta)))
             return False
 
         return True
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 6f0aa77e..e8976f98 100755
--- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
+++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
@@ -82,7 +82,7 @@ def reader_thread(self):
             packet = self.comms.read_packet()
             # print("trying to read odom")
             if isinstance(packet, Odometry) and self_name == "SC":
-                rospy.logdebug("packet", packet)
+                rospy.logdebug("packet" + str(packet))
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
                 # convert to long lat
@@ -92,10 +92,10 @@ def reader_thread(self):
                     odom.pose.pose.position.y = lat
                     self.odom_publisher.publish(odom)
                 except Exception as e:
-                    rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
+                    rospy.logwarn("Unable to convert other buggy position to lon lat" + str(e))
 
             elif isinstance(packet, BnyaTelemetry) and self_name == "NAND":
-                rospy.logdebug("packet", packet)
+                rospy.logdebug("packet" + str(packet))
                 odom = ROSOdom()
                 try:
                     lat, long = World.utm_to_gps(packet.y, packet.x)
@@ -106,7 +106,7 @@ def reader_thread(self):
 
                     self.odom_publisher.publish(odom)
                 except Exception as e:
-                    rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
+                    rospy.logwarn("Unable to convert other buggy position to lon lat" + str(e))
 
 
             elif isinstance(packet, tuple): #Are there any other packet that is a tuple