Velocity port done

rb_ws/environments/docker_env.bash

@@ -1,4 +1,5 @@
 #!/bin/sh
 export RBROOT=/rb_ws
 export PYTHONPATH=$PYTHONPATH:$RBROOT/src/buggy/scripts
-export TRAJPATH=$RBROOT/src/buggy/paths/
+export TRAJPATH=$RBROOT/src/buggy/paths/
+export VELPATH=$RBROOT/src/buggy/scripts/simulator/velocity_checkpoints/

rb_ws/src/buggy/CMakeLists.txt

@@ -31,6 +31,7 @@ install(PROGRAMS
   scripts/controller/controller_node.py
   scripts/path_planner/path_planner.py
   scripts/simulator/engine.py
+  scripts/simulator/velocity_updater.py
   scripts/watchdog/watchdog_node.py
   scripts/buggy_state_converter.py
   scripts/serial/ros_to_bnyahaj.py
@@ -51,4 +52,4 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 )
 ament_export_dependencies(rosidl_default_runtime)
 
-ament_package()
+ament_package()

rb_ws/src/buggy/config/sim_single_velocity.yaml

@@ -0,0 +1,30 @@
+/**: # Global Params
+  ros__parameters:
+      traj_name: "buggycourse_safe.json"
+      checkpoints_name : "buggycourse_safe_checkpoints_1.json"
+
+
+SC:
+  SC_sim_single:
+    ros__parameters:
+      velocity: 12
+      pose: "Hill1_SC"
+
+SC:
+  SC_controller:
+    ros__parameters:
+      dist: 0.0
+      # traj_name: "buggycourse_safe.json"
+      controller: "stanley"
+
+SC:
+  SC_path_planner:
+    ros__parameters:
+      traj_name: "buggycourse_safe.json"
+      curb_name: "buggycourse_curb.json"
+
+SC:
+  SC_velocity_updater:
+    ros__parameters:
+      init_vel: 12
+      checkpoints_name : "buggycourse_safe_checkpoints_1.json"

rb_ws/src/buggy/launch/sim_2d_single.xml

@@ -1,16 +1,18 @@
 <launch>
 
-    <arg name="config_file" default="src/buggy/config/sim_single_sc.yaml"/>
-
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-
-    <node pkg="buggy" exec="engine.py" name="SC_sim_single" namespace="SC">
-        <param from="$(var config_file)"/>
+    <node pkg="buggy" exec="hello_world" name="hello_world" namespace="SC" />
+    <node pkg="buggy" exec="sim_single" name="SC_sim_single" namespace="SC">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_SC"/>
     </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">
-        <param from="$(var config_file)"/>
+    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_NAND"/>
     </node>
 
+    <!-- <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
+    <node name="hello_world" pkg="buggy" type="hello_world" /> -->
+
     <node pkg="buggy" exec="watchdog_node.py" name="SC_watchdog" namespace="SC"/>
 </launch>

rb_ws/src/buggy/launch/sim_2d_single_velocity.xml

@@ -0,0 +1,21 @@
+<launch>
+
+    <arg name="config_file" default="src/buggy/config/sim_single_velocity.yaml"/>
+
+    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" 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 from="$(var config_file)"/>
+    </node>
+    <node pkg="buggy" exec="engine.py" name="SC_sim_single" 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="velocity_updater.py" name="SC_velocity_updater" namespace="SC">
+        <param from="$(var config_file)"/>
+    </node>
+
+</launch>

rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/buggycourse_safe_checkpoints_1.json

@@ -0,0 +1,17 @@
+{
+    "checkpoints": 
+    [
+        {
+            "x-pos": 589700,
+            "y-pos": 4477159,
+            "radius": 20,
+            "velocity": 12.345
+        },
+        {
+            "x-pos": 589685,
+            "y-pos": 4477153,
+            "radius": 20,
+            "velocity": 17.234
+        }
+    ]
+}

rb_ws/src/buggy/scripts/simulator/velocity_checkpoints/checkpoints_explained.txt

@@ -0,0 +1,5 @@
+Each checkpoints file have a corresponding traj file. Each traj file may have multiple checkpoints file.
+Each checkpoints file also needs explaination and comments on the source of data. That is what this file does.\
+
+Checkpoints File                        | Corresponding Traj File       | Data Source and/or comments
+buggycourse_safe_checkpoints_1.json     | buggycourse_safe.json         | Nonsense data for testing only with on decrease and than increase in velocity

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

@@ -0,0 +1,101 @@
+#! /usr/bin/env python3
+import os
+import math
+import threading
+import json
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+from std_msgs.msg import Float64
+
+class VelocityUpdater(Node):
+    RATE = 100
+
+    def __init__(self):
+        super().__init__('velocity_updater')
+        self.get_logger().info('INITIALIZED.')
+
+        # Bubbles for updating acceleration based on position
+        # represented as {x-pos, y-pos, radius, velocity}
+        # imported from a json file corresponding to the path
+        self.declare_parameter("checkpoints_name", "buggycourse_safe_checkpoints_1.json")
+        checkpoints_name = self.get_parameter("checkpoints_name").value
+        checkpoints_path = os.environ["VELPATH"] + checkpoints_name
+        with open(checkpoints_path, 'r') as checkpoints_file:
+            self.CHECKPOINTS = (json.load(checkpoints_file))["checkpoints"]
+
+        # Declare parameters with default values
+        self.declare_parameter('init_vel', 12)
+        self.declare_parameter('buggy_name', 'SC')
+        # Get the parameter values
+        self.init_vel = self.get_parameter("init_vel").value
+        self.buggy_name = self.get_parameter("buggy_name").value
+
+        # initialize variables
+        self.buggy_vel = self.init_vel
+        self.debug_dist = 0
+
+        self.position = Point()
+        self.lock = threading.Lock()
+
+        # subscribe sim_2d/utm for position values
+        self.pose_subscriber = self.create_subscription(
+            Pose,
+            "sim_2d/utm",
+            self.update_position,
+            1
+        )
+
+        # publish velocity to "sim/velocity"
+        self.velocity_publisher = self.create_publisher(Float64, "sim/velocity", 1)
+
+        # ROS2 timer for stepping
+        self.timer = self.create_timer(1.0 / self.RATE, self.step)
+
+    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 check_velocity(self):
+        '''Check if the position of the buggy is in any of the checkpoints set
+        in self.CHECKPOINTS, and update velocity of buggy accordingly
+        '''
+        for checkpoint in self.CHECKPOINTS:
+            x = checkpoint["x-pos"]
+            y = checkpoint["y-pos"]
+            r = checkpoint["radius"]
+            v = checkpoint["velocity"]
+            dist = math.sqrt((x-self.position.x)**2 + (y-self.position.y)**2)
+            self.debug_dist = dist
+            if dist < r:
+                self.buggy_vel = v
+                break
+
+    def step(self):
+        '''Update velocity of buggy for one timestep
+        and publish it so that the simulator engine can subscribe and use it'''
+
+        # update velocity
+        self.check_velocity()
+
+        # publish velocity
+        float_64_velocity = Float64()
+        float_64_velocity.data = float(self.buggy_vel)
+        self.velocity_publisher.publish(float_64_velocity)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    vel_updater = VelocityUpdater()
+    rclpy.spin(vel_updater)
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()