the dots move now

Author: TiaSinghania

docker_tester

@@ -0,0 +1,13 @@
+matplotlib==3.1.2
+NavPy==1.0
+numba==0.58.0
+numpy<1.21.0
+osqp==0.6.3
+pandas==2.0.3
+pyembree==0.2.11
+pymap3d==3.0.1
+scipy==1.10.1
+trimesh==3.23.5
+utm==0.7.0
+keyboard==0.13.5
+tk==0.1.0

rb_ws/src/buggy/buggy/simulator/engine.py

@@ -13,16 +13,17 @@
 
 class Simulator(Node):
     # simulator constants:
+    UTM_EAST_ZERO = 589702.87
+    UTM_NORTH_ZERO = 4477172.947
+    UTM_ZONE_NUM = 17
+    UTM_ZONE_LETTER = "T"
+    #TODO: make these values accurate
+    WHEELBASE_SC = 1.17
+    WHEELBASE_NAND= 1.17
 
     def __init__(self):
         super().__init__('sim_single')
-        UTM_EAST_ZERO = 589702.87
-        UTM_NORTH_ZERO = 4477172.947
-        UTM_ZONE_NUM = 17
-        UTM_ZONE_LETTER = "T"
-        #TODO: make these values accurate
-        WHEELBASE_SC = 1.17
-        WHEELBASE_NAND= 1.17
+        self.get_logger().info('INITIALIZED.')
 
         self.starting_poses = {
             "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
@@ -60,19 +61,22 @@ def __init__(self):
         )
         if (self.get_namespace() == "/SC"):
             self.buggy_name = "SC"
-            init_pose = self.starting_poses["Hill_1SC"]
-
+            init_pose = self.starting_poses["Hill1_SC"]
+            self.wheelbase = Simulator.WHEELBASE_SC
 
         if (self.get_namespace() == "/NAND"):
             self.buggy_name = "NAND"
-            init_pose = self.starting_poses["Hill_1NAND"]
+            init_pose = self.starting_poses["Hill1_NAND"]
+            self.wheelbase = Simulator.WHEELBASE_NAND
+
 
         self.e_utm, self.n_utm, self.heading = init_pose
         # TODO: do we want to not hard code this
         self.velocity = 12 # m/s
         self.steering_angle = 0  # degrees
         self.rate = 200  # Hz
         self.pub_skip = 2  # publish every pub_skip ticks
+        self.pub_tick_count = 0
 
         self.lock = threading.Lock()
 
@@ -97,7 +101,7 @@ def get_steering_arc(self):
         return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
 
     def dynamics(self, state, v):
-        l = Simulator.WHEELBASE
+        l = self.wheelbase
         _, _, theta, delta = state
 
         return np.array([v * np.cos(theta),
@@ -151,49 +155,50 @@ def publish(self):
         nsf = NavSatFix()
         nsf.latitude = lat
         nsf.longitude = long
-        nsf.altitude = 260
+        nsf.altitude = float(260)
         nsf.header.stamp = time_stamp
-        self.navsatfix_publisher.publish(nsf)
 
-        if Simulator.NOISE:
-            lat_noisy = lat + np.random.normal(0, 1e-6)
-            long_noisy = long + np.random.normal(0, 1e-6)
-            nsf_noisy = NavSatFix()
-            nsf_noisy.latitude = lat_noisy
-            nsf_noisy.longitude = long_noisy
-            nsf_noisy.header.stamp = time_stamp
-            self.navsatfix_noisy_publisher.publish(nsf_noisy)
+        lat_noisy = lat + np.random.normal(0, 1e-6)
+        long_noisy = long + np.random.normal(0, 1e-6)
+        nsf_noisy = NavSatFix()
+        nsf_noisy.latitude = lat_noisy
+        nsf_noisy.longitude = long_noisy
+        nsf_noisy.header.stamp = time_stamp
+        self.navsatfix_noisy_publisher.publish(nsf_noisy)
 
         odom = Odometry()
         odom.header.stamp = time_stamp
 
         odom_pose = Pose()
-        odom_pose.position.x = long
-        odom_pose.position.y = lat
-        odom_pose.position.z = 260
+        odom_pose.position.x = float(long)
+        odom_pose.position.y = float(lat)
+        odom_pose.position.z = float(260)
 
         odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2)
         odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2)
 
         odom_twist = Twist()
-        odom_twist.linear.x = velocity
+        odom_twist.linear.x = float(velocity)
 
         odom.pose = PoseWithCovariance(pose=odom_pose)
         odom.twist = TwistWithCovariance(twist=odom_twist)
 
         self.pose_publisher.publish(odom)
 
     def loop(self):
-        rate = self.create_rate(self.rate)
-        pub_tick_count = 0
+        msg = Float64()
+        msg.data = float(self.i)
 
-        self.step()
+        self.number_publisher.publish(msg)
+        self.i += 1
 
-        if pub_tick_count == self.pub_skip:
+        self.step()
+        if self.pub_tick_count == self.pub_skip:
             self.publish()
-            pub_tick_count = 0
+            self.pub_tick_count = 0
         else:
-            pub_tick_count += 1
+            self.pub_tick_count += 1
+
 
 
 

rb_ws/src/buggy/scripts/simulator/engine.py

@@ -0,0 +1,32 @@
+#! /usr/bin/env python3
+import sys
+import time
+import rclpy
+from std_msgs.msg import String
+
+class Simulator():
+    # simulator constants:
+
+    def __init__(self):
+        self.test_publisher = self.create_publisher(String, 'test', 10)
+        if (self.get_namespace() == "SC"):
+            self.buggy_name = "SC"
+
+        if (self.get_namespace() == "NAND"):
+            self.buggy_name = "NAND"
+
+    def loop(self):
+        print("hello")
+        self.test_publisher.publish(self.buggy_name)
+
+if __name__ == "__main__":
+    rclpy.init()
+    sim = Simulator()
+
+    # publish initial position, then sleep
+    # so that auton stack has time to initialize
+    # before buggy moves
+    time.sleep(15.0)
+    sim.loop()
+
+