fix logging, set default controller to stanley

CMU-Robotics-Club · Apr 6, 2024 · 29867d8 · 29867d8
1 parent 4672be8
commit 29867d8
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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