mock rolls 1/29

Author: cmuroboclub

.gitignore

@@ -9,7 +9,7 @@ docker-compose.yml~
 rb_ws/bags
 rb_ws/src/buggy/bags/                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
 *.bag
-rb_ws/rosbag2/
+rb_ws/rosbag2*/
 
 # VISION
 .vision/

rb_ws/src/buggy/CMakeLists.txt

@@ -34,7 +34,7 @@ install(PROGRAMS
   scripts/watchdog/watchdog_node.py
   scripts/buggy_state_converter.py
   scripts/serial/ros_to_bnyahaj.py
-  scripts/visualization/telematics.py
+  scripts/telemetry/telematics.py
   scripts/debug/debug_steer.py
   DESTINATION lib/${PROJECT_NAME}
 )

rb_ws/src/buggy/config/sc-mockroll.yaml

@@ -1,6 +1,6 @@
 /**: # Global Params
   ros__parameters:
-      traj_name: "cut_square.json"
+      traj_name: "UC_to_purnell.json"
 
 SC:
   bnyahaj:

rb_ws/src/buggy/launch/sc-main.xml

@@ -1,7 +1,9 @@
-<arg name="config_file" default="src/buggy/config/sc-mockroll.yaml"/>
-<node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
+<launch>
+    <arg name="config_file" default="src/buggy/config/sc-mockroll.yaml"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
+            <param from="$(var config_file)"/>
+    </node>
+    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
         <param from="$(var config_file)"/>
-</node>
-<node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
-    <param from="$(var config_file)"/>
-</node>
+    </node>
+</launch>

rb_ws/src/buggy/launch/sc-system.xml

@@ -6,10 +6,10 @@
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" namespace="SC"/>
 
-    <node name="bnyahaj" pkg="buggy" exec="ros_to_bnyahaj.py" respawn="true" output="screen", namespace="SC">
+    <node name="bnyahaj" pkg="buggy" exec="ros_to_bnyahaj.py" respawn="true" output="screen" namespace="SC">
         <param from="$(var config_file)"/>
     </node>
-    <node name="stateconverter" pkg = "buggy" exec = "buggy_state_converter.py" output="screen", namespace="SC"/>
+    <node name="stateconverter" pkg = "buggy" exec = "buggy_state_converter.py" output="screen" namespace="SC"/>
 
     <node name="telematics" pkg="buggy" exec="telematics.py" namespace="SC" output="screen"/>
     <node name="watchdog" pkg="buggy" exec="watchdog_node.py" namespace="SC" output="screen"/>

rb_ws/src/buggy/scripts/controller/controller_node.py

@@ -100,8 +100,8 @@ def init_check(self):
             self.get_logger().warn("WARNING: no available position estimate")
             return False
 
-        elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2):
-            self.get_logger().warn("checking position estimate certainty")
+        elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1):
+            self.get_logger().warn("checking position estimate certainty | current covariance: " + str(self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 ))
             return False
 
         #Originally under a lock, doesn't seem necessary?

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

@@ -44,9 +44,9 @@ def __init__(self):
         self.lock = Lock()
 
         self.create_subscription(
-            Float64, "/input/steering", self.set_steering, 1
+            Float64, "input/steering", self.set_steering, 1
         )
-        self.create_subscription(Int8, "/input/sanity_warning", self.set_alarm, 1)
+        self.create_subscription(Int8, "input/sanity_warning", self.set_alarm, 1)
 
         # upper bound of reading data from Bnyahaj Serial, at 1ms
         self.timer = self.create_timer(0.001, self.loop)
@@ -180,7 +180,7 @@ def loop(self):
 
             elif isinstance(packet, RoundtripTimestamp):
 
-                self.get_logger().info(f'Roundtrip Timestamp: {packet.returned_time}, {(time.time_ns() * 1e-6 - packet.returned_time) * 1e-3}')
+                self.get_logger().debug(f'Roundtrip Timestamp: {packet.returned_time}, {(time.time_ns() * 1e-6 - packet.returned_time) * 1e-3}')
                 self.roundtrip_time_publisher.publish(Float64(data=(time.time_ns() * 1e-6 - packet.returned_time) * 1e-3))
 
         if self.fresh_steer:
@@ -202,4 +202,7 @@ def main(args=None):
     rclpy.spin(translator)
 
     translator.destroy_node()
-    rclpy.shutdown()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

rb_ws/src/buggy/scripts/telemetry/telematics.py

@@ -22,11 +22,11 @@ def __init__(self):
         def wrap_args(callback, callback_args):
             return lambda msg: callback(msg, callback_args)
 
-        self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10)
-        self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1)
+        self.self_publisher = self.create_publisher(NavSatFix, "self/state_navsatfix", 10)
+        self.self_subscriber = self.create_subscription(Odometry, "self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1)
 
-        self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10)
-        self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1)
+        self.other_publisher = self.create_publisher(NavSatFix, "other/state_navsatfix", 10)
+        self.other_subscriber = self.create_subscription(Odometry, "other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1)
 
     # TODO Make this a static method?
     def convert_buggystate(self, msg, publisher):