Truck weekend (#87)

* fix logging, set default controller to stanley * set respawn=true for bnyaserial * 4_6 roll * Fix message parsing (#85) (#86) Co-authored-by: SuperTails <[email protected]> --------- Co-authored-by: Jackack <[email protected]> Co-authored-by: Buggy <[email protected]> Co-authored-by: Jack He <[email protected]> Co-authored-by: SuperTails <[email protected]>
CMU-Robotics-Club · Apr 6, 2024 · 1a398d0 · 1a398d0
1 parent f3a4c59
commit 1a398d0
Show file tree

Hide file tree

Showing 7 changed files with 302 additions and 16 deletions.
diff --git a/rb_ws/src/buggy/launch/nand-system.launch b/rb_ws/src/buggy/launch/nand-system.launch
@@ -2,7 +2,7 @@
 
 <launch>
 
-    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" output="screen" args="--self_name NAND --teensy_name nand-teensy"/>
+    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" respawn="true" output="screen" args="--self_name NAND --teensy_name nand-teensy"/>
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />

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" />
 
@@ -11,7 +11,7 @@
     <!-- ENABLE AUTON -->
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <!-- Conditional Launch Files, depending on if NAND Exists or not -->
-    <arg name="NAND_exist" default="false"/>
+    <arg name="NAND_exist" default="true"/>
     <group if="$(arg NAND_exist)">
         <!-- Run the simulation with NAND -->
         <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND --left_curb curb.json" />

diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch
@@ -5,7 +5,7 @@
         <arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
     </include>
 
-    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" output="screen" args="--self_name SC --other_name NAND --teensy_name ttyUSB0"/>
+    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" respawn="true" output="screen" args="--self_name SC --other_name NAND --teensy_name ttyUSB0"/>
 
     <node name="serial_node2" pkg="rosserial_python" type="serial_node.py">
         <param name="port" type="string" value="/dev/ttyACM1"/>

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/paths/square.json b/rb_ws/src/buggy/paths/square.json
@@ -0,0 +1,284 @@
+[
+    {
+      "key": "fdc801c8-d657-4d1b-9cda-384e8c99466e",
+      "lat": 40.442864590935415,
+      "lon": -79.94270383609089,
+      "active": false
+    },
+    {
+      "key": "9673074e-075d-45e2-ba4c-91b26e64ea1f",
+      "lat": 40.442818017581345,
+      "lon": -79.94274193874543,
+      "active": false
+    },
+    {
+      "key": "5cd4b45c-51a4-4e4a-88cf-3d181b9e146d",
+      "lat": 40.44277485038435,
+      "lon": -79.9427702988864,
+      "active": false
+    },
+    {
+      "key": "96f68380-5051-4cfe-9083-d876321f963a",
+      "lat": 40.44272132302189,
+      "lon": -79.94281000308416,
+      "active": false
+    },
+    {
+      "key": "66cb1621-b5db-4843-bc36-45f7f4dd1480",
+      "lat": 40.442664342234075,
+      "lon": -79.94284290084782,
+      "active": false
+    },
+    {
+      "key": "d7199397-8220-478e-9cdf-66e55ead881d",
+      "lat": 40.44262031159218,
+      "lon": -79.94286105133817,
+      "active": false
+    },
+    {
+      "key": "28543aa5-95b2-43df-a52a-51fc494d2cc2",
+      "lat": 40.44256246737186,
+      "lon": -79.94288033623401,
+      "active": false
+    },
+    {
+      "key": "4c6b1f1c-26ad-491a-944e-abe1f961235f",
+      "lat": 40.44251325657793,
+      "lon": -79.94288941147913,
+      "active": false
+    },
+    {
+      "key": "5ab3d8aa-8899-4303-824c-e3a7a23947f1",
+      "lat": 40.44245800231049,
+      "lon": -79.94290075553533,
+      "active": false
+    },
+    {
+      "key": "719bc4dd-258b-42fd-9117-7a35e201e3f0",
+      "lat": 40.442410518138075,
+      "lon": -79.9429120995918,
+      "active": false
+    },
+    {
+      "key": "c3e59fa5-aac2-4e9b-87c6-88dda139cb44",
+      "lat": 40.44236476063086,
+      "lon": -79.94291777162007,
+      "active": false
+    },
+    {
+      "key": "5cbd11ba-154d-45d2-89d3-89991463b987",
+      "lat": 40.44231813974269,
+      "lon": -79.94292571245961,
+      "active": false
+    },
+    {
+      "key": "4e8fb9de-ec5a-4c79-bf2e-f4b38d9009cc",
+      "lat": 40.44228533243158,
+      "lon": -79.94292798127083,
+      "active": false
+    },
+    {
+      "key": "0528884d-6a0e-42df-a628-bf9b66054609",
+      "lat": 40.44223266803022,
+      "lon": -79.94294045973312,
+      "active": false
+    },
+    {
+      "key": "96555298-2411-45ba-932f-2b3b06022971",
+      "lat": 40.44218950045731,
+      "lon": -79.94294953497833,
+      "active": false
+    },
+    {
+      "key": "420e4f7f-196d-4ae0-a0a6-11968fe54d0b",
+      "lat": 40.442162736548276,
+      "lon": -79.94295974462918,
+      "active": false
+    },
+    {
+      "key": "f10aa65e-88fd-4e43-95a9-f34bd31ebaa2",
+      "lat": 40.44215064961804,
+      "lon": -79.942979029525,
+      "active": false
+    },
+    {
+      "key": "da5e9184-5af9-424c-9e8c-467584d2b7e5",
+      "lat": 40.44214028939062,
+      "lon": -79.94300398644916,
+      "active": false
+    },
+    {
+      "key": "ae25473f-38ae-4f3c-bab1-3b76869b4072",
+      "lat": 40.44213769933332,
+      "lon": -79.94303461540154,
+      "active": false
+    },
+    {
+      "key": "f9f44014-99d9-4abe-93d7-e70379ec3c65",
+      "lat": 40.44213601038151,
+      "lon": -79.94306530188086,
+      "active": false
+    },
+    {
+      "key": "417cef7e-fe83-4525-9b34-1bb2c77dd856",
+      "lat": 40.442135498325435,
+      "lon": -79.94309625152094,
+      "active": false
+    },
+    {
+      "key": "f664b8b1-8e2b-41bc-82c8-c679816de991",
+      "lat": 40.44213669312284,
+      "lon": -79.94312271570611,
+      "active": false
+    },
+    {
+      "key": "cfd4138f-c95c-4570-8f9f-db3302fe59b3",
+      "lat": 40.44214078957061,
+      "lon": -79.94314895561843,
+      "active": false
+    },
+    {
+      "key": "505c4b07-d1c4-4e58-ad78-e108cfbf1f8b",
+      "lat": 40.44214488601873,
+      "lon": -79.94317855962205,
+      "active": false
+    },
+    {
+      "key": "232c4793-c8c6-437c-b802-147ef6a8fa6d",
+      "lat": 40.4421522254874,
+      "lon": -79.94321040635322,
+      "active": false
+    },
+    {
+      "key": "3432c089-ec84-40c5-9e75-73689c4f8a3f",
+      "lat": 40.442160930437936,
+      "lon": -79.94325256963127,
+      "active": false
+    },
+    {
+      "key": "6d8633d9-9ac7-4749-8535-c6a554d80512",
+      "lat": 40.442173731833314,
+      "lon": -79.94330818927436,
+      "active": false
+    },
+    {
+      "key": "4fac6e23-57e7-489f-a011-d9e18c668b8a",
+      "lat": 40.44218585048538,
+      "lon": -79.9433629118265,
+      "active": false
+    },
+    {
+      "key": "4e945755-bd48-40ec-9cd3-995bba213d6c",
+      "lat": 40.44222231109141,
+      "lon": -79.94355183188163,
+      "active": false
+    },
+    {
+      "key": "4f3212bd-b8ab-4f7a-b4d8-bbdeb1359018",
+      "lat": 40.44224210860298,
+      "lon": -79.94368785852456,
+      "active": false
+    },
+    {
+      "key": "b3429fac-2e0d-4dc2-a7f2-141b6ce4e542",
+      "lat": 40.442257322135355,
+      "lon": -79.94371433935326,
+      "active": false
+    },
+    {
+      "key": "6b206f08-ad6f-45cb-8e76-eddf894a4125",
+      "lat": 40.44227723255511,
+      "lon": -79.94372305983086,
+      "active": false
+    },
+    {
+      "key": "4e95379c-2428-4b74-9fb9-19fd59e5e17b",
+      "lat": 40.44230175987565,
+      "lon": -79.94371851001644,
+      "active": false
+    },
+    {
+      "key": "40f8cc48-d91b-464b-8e29-dadb29fb3acb",
+      "lat": 40.442325421517424,
+      "lon": -79.9437003107587,
+      "active": false
+    },
+    {
+      "key": "4afaf571-d8f7-4e91-9ebb-28079a0e5e0d",
+      "lat": 40.44237072488155,
+      "lon": -79.94367718253554,
+      "active": false
+    },
+    {
+      "key": "3d6ad703-0fa6-4eb4-92b1-75bedc3687ca",
+      "lat": 40.44243997837277,
+      "lon": -79.94364609213703,
+      "active": false
+    },
+    {
+      "key": "cfa3f96b-94db-4797-b64a-26e4e10f6785",
+      "lat": 40.44259582399275,
+      "lon": -79.9435729097322,
+      "active": false
+    },
+    {
+      "key": "b464e03a-be2d-488c-8744-e0e6e819bb6b",
+      "lat": 40.442706193311764,
+      "lon": -79.9435282473586,
+      "active": false
+    },
+    {
+      "key": "f38f53f3-6c69-4822-b8cf-f100dede4a33",
+      "lat": 40.44282416031863,
+      "lon": -79.94349356833904,
+      "active": false
+    },
+    {
+      "key": "23f54818-7ff2-4147-88a8-73a0b7922c21",
+      "lat": 40.44293212994028,
+      "lon": -79.94349409377874,
+      "active": false
+    },
+    {
+      "key": "f4b3a40d-0bbc-4156-b690-17d45728f75b",
+      "lat": 40.44303925949717,
+      "lon": -79.94349924891674,
+      "active": false
+    },
+    {
+      "key": "6e9cb84a-0166-4b3b-a45d-288b2c7c4713",
+      "lat": 40.44309359015794,
+      "lon": -79.94348518744819,
+      "active": false
+    },
+    {
+      "key": "06c702b7-4d02-4cd1-9ea9-770f7658432a",
+      "lat": 40.44311663951579,
+      "lon": -79.9434430030424,
+      "active": false
+    },
+    {
+      "key": "67e65a27-58a3-4107-85c7-50481996a692",
+      "lat": 40.44311499313339,
+      "lon": -79.94339541037948,
+      "active": false
+    },
+    {
+      "key": "d0f83bb4-d974-4de0-95d3-65c456fbb2a8",
+      "lat": 40.44309688292377,
+      "lon": -79.94330563331079,
+      "active": false
+    },
+    {
+      "key": "3ddba30d-63e6-4522-aaa1-928179f3f05c",
+      "lat": 40.443054900146485,
+      "lon": -79.94312175256749,
+      "active": false
+    },
+    {
+      "key": "3743b0bf-d569-4581-90d1-7bde32d1e6ce",
+      "lat": 40.44300468541742,
+      "lon": -79.94289136081294,
+      "active": false
+    }
+  ]
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