Sw/update velocity

rb_ws/src/buggy/launch/sim_2d.launch

@@ -6,12 +6,14 @@
     <arg name="controller"      default="stanley" />
     <arg name="start_dist"      default="0.0" />
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
-    <node name="mpc_calculator" pkg="buggy" type="model_predictive_controller.py" output="screen"/>
+    <!-- <node name="mpc_calculator" pkg="buggy" type="model_predictive_controller.py" output="screen"/> -->
 
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
     <!-- <node name="sim_2d_visualizer" pkg="buggy" type="grapher.py" output="screen"/> -->
     <node name="sim_2d_controller" pkg="buggy" type="controller_2d.py" output="screen"/>
 
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg controller) $(arg start_dist)"/>
+
+    <node name="velocity_updater" pkg="buggy" type="velocity_updater.py" output="screen"/>
 </launch>

rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py

@@ -0,0 +1,62 @@
+#! /usr/bin/env python3
+import rospy
+from controller_2d import Controller
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+import threading
+import math
+
+class VelocityUpdater:
+    RATE = 100
+    CHECKPOINTS = [
+        (589701, 4477160, 20, 0.5)
+    ]
+
+    def __init__(self):
+        self.pose_subscriber = rospy.Subscriber(
+            "sim_2d/utm", Pose, self.update_position
+        )
+
+        self.buggy_vel = 15.0
+        self.time = 0.0
+        self.accel = 0.0
+
+        self.position = Point()
+        self.position.x = 0
+        self.position.y = 0
+        self.position.z = 0
+
+        self.controller = Controller()
+
+        self.lock = threading.Lock()
+
+    def update_position(self, new_pose: Pose):
+        '''Callback function to update internal position variable'''
+        with self.lock:
+            self.position = new_pose.position
+
+    def calculate_accel(self):
+        for (x,y,r,a) in self.CHECKPOINTS:
+            dist = math.sqrt((x - self.position.x)**2 + (y - self.position.y)**2)
+            if dist < r:
+                self.accel = a
+
+    def step(self):
+        self.calculate_accel()
+
+        new_velocity = self.buggy_vel + self.accel / self.RATE
+        self.buggy_vel = new_velocity
+        self.controller.set_velocity(new_velocity)
+
+        self.time += 1 / self.RATE
+
+
+if __name__ == "__main__":
+    rospy.init_node("velocity_updater")
+    vel = VelocityUpdater()
+    rate = rospy.Rate(vel.RATE)
+
+    while not rospy.is_shutdown():
+        vel.step()
+        rate.sleep()

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -157,7 +157,7 @@ def tick(self):
     print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n")
     print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n")
 
-    trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json")
+    trajectory = Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe_1.json")
     # calculate starting index
     start_index = trajectory.get_index_from_distance(start_dist)