updated engine to have a small delay

CMU-Robotics-Club · Jan 7, 2025 · c9c17d1 · c9c17d1
1 parent 1979842
commit c9c17d1
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 9 deletions.
diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml
@@ -1,29 +1,29 @@
 <launch>
 
-    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
+    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" namespace="SC"/>
 
-    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" namespace="NAND"/>
-    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" namespace="NAND">
+    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" output = "screen" namespace="NAND"/>
+    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" output = "screen" namespace="NAND">
         <param name="dist" value="0.0"/>
         <param name="traj_name" value="buggycourse_safe.json"/>
         <param name="controller" value="stanley"/>
     </node>
-    <node pkg="buggy" exec="engine.py" name="NAND_sim_single" namespace="NAND">
+    <node pkg="buggy" exec="engine.py" name="NAND_sim_single" output = "screen" namespace="NAND">
         <param name="velocity" value="8"/>
         <param name="pose" value="Hill1_NAND"/>
     </node>
 
-    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" namespace="SC"/>
-    <node pkg="buggy" exec="controller_node.py" name="SC_controller" namespace="SC">
+    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" output = "screen" namespace="SC"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
         <param name="dist" value="0.0"/>
         <param name="traj_name" value="buggycourse_safe.json"/>
         <param name="controller" value="stanley"/>
     </node>
-    <node pkg="buggy" exec="engine.py" name="SC_sim_single" namespace="SC">
+    <node pkg="buggy" exec="engine.py" name="SC_sim_single" output = "screen" namespace="SC">
         <param name="velocity" value="12"/>
         <param name="pose" value="Hill1_SC"/>
     </node>
-    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output="screen">
+    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
         <param name="traj_name" value="buggycourse_safe.json"/>
         <remap from="other/state" to="/NAND/self/state"/>
     </node>

diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py
@@ -9,6 +9,8 @@
 from nav_msgs.msg import Odometry
 import numpy as np
 import utm
+import time
+
 sys.path.append("/rb_ws/src/buggy/scripts")
 from util.constants import Constants
 
@@ -130,7 +132,7 @@ def publish(self):
         with self.lock:
             p.position.x = self.e_utm
             p.position.y = self.n_utm
-            p.position.z = self.heading
+            p.position.z = float(self.heading)
             velocity = self.velocity
 
         self.plot_publisher.publish(p)
@@ -182,6 +184,12 @@ def loop(self):
 def main(args=None):
     rclpy.init(args=args)
     sim = Simulator()
+    for i in range(500):
+        time.sleep(0.01)
+        sim.publish()
+
+
+    sim.get_logger().info("STARTED PUBLISHING")    
     rclpy.spin(sim)
 
     sim.destroy_node()