Sw/update velocity (#34)

* Create velocity_updater.py

Basic framework of VelocityUpdater class, just applies constant acceleration to the buggy

* Add velocity_updater node

Added velocity_updater node to launch file

* Comment out mpc node

Removed mpc_calculator node from launch file to remove terminal error message when running simulation

* Renamed VelocityUpdater attributes

* Method to vary acceleration w/ time

Added calculate_accel function to vary velocity over time (currently accelerates for 20s and then stops)

* Change which trajectory is used for simulation

* Change to set buggy velocity through Controller

* Add subscriber to access position of buggy

* Add position-based acceleration

* Add comments and remove self.time

* Added velocity updater to new launch files

* Fixed some small errors

Velocity updater code now works for both simulating single buggies and 2 buggies

rb_ws/src/buggy/launch/sim_2d_2buggies.launch

@@ -19,13 +19,18 @@
 
     <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/>
+    <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
+        output="screen" args="$(arg sc_velocity) SC"/>
 
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg sc_controller) $(arg sc_start_dist) $(arg sc_path) SC True"/>
 
     <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen" 
         args="$(arg nand_starting_pose) $(arg nand_velocity) Nand"/>
+    <node name="nand_vel_updater" pkg="buggy" type="velocity_updater.py"
+        output="screen" args="$(arg nand_velocity) Nand"/>
+
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg nand_controller) $(arg nand_start_dist) $(arg nand_path) Nand True"/>
 </launch>

rb_ws/src/buggy/launch/sim_2d_single.launch

@@ -14,6 +14,8 @@
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
+    <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
+        output="screen" args="$(arg velocity) $(arg buggy_name)"/>
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>

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

@@ -0,0 +1,74 @@
+#! /usr/bin/env python3
+import rospy
+import sys
+from controller_2d import Controller
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+import threading
+import math
+
+class VelocityUpdater:
+    RATE = 100
+    # Bubbles for updating acceleration based on position
+    # represented as 4-tuples: (x-pos, y-pos, radius, acceleration)
+    CHECKPOINTS: 'list[tuple[float,float,float,float]]' = [
+        (589701, 4477160, 20, 0.5)
+    ]
+
+    def __init__(self, init_vel: float, buggy_name: str):
+        self.pose_subscriber = rospy.Subscriber(
+            buggy_name + "/sim_2d/utm", Pose, self.update_position
+        )
+
+        self.buggy_vel = init_vel
+        self.accel = 0.0
+
+        self.position = Point()
+        self.position.x = 0
+        self.position.y = 0
+        self.position.z = 0
+
+        self.controller = Controller(buggy_name)
+
+        self.lock = threading.Lock()
+
+    def update_position(self, new_pose: Pose):
+        '''Callback function to update internal position variable when new
+        buggy position is published
+        
+        Args:
+            new_pose (Pose): Pose object from topic
+        '''
+        with self.lock:
+            self.position = new_pose.position
+
+    def calculate_accel(self):
+        '''Check if the position of the buggy is in any of the checkpoints set
+        in self.CHECKPOINTS, and update acceleration of buggy accordingly
+        '''
+        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
+                break
+
+    def step(self):
+        '''Update acceleration and velocity of buggy for one timestep'''
+        self.calculate_accel()
+
+        new_velocity = self.buggy_vel + self.accel / self.RATE
+        self.buggy_vel = new_velocity
+        self.controller.set_velocity(new_velocity)
+
+
+if __name__ == "__main__":
+    rospy.init_node("vel_updater")
+
+    init_vel = float(sys.argv[1])
+    buggy_name = sys.argv[2]
+    vel = VelocityUpdater(init_vel, buggy_name)
+    rate = rospy.Rate(vel.RATE)
+
+    while not rospy.is_shutdown():
+        vel.step()
+        rate.sleep()