Launch files

safety_controller/README.md

@@ -0,0 +1,4 @@
+Safety_controller subscribes to ouster_points and imu_packets.
+First, projects car's position in the next 1 second.
+Second, takes the linear regression between curr position and future position.
+Third, calculates distance from lidar points to line. If within crash delta (.2 m), then publishes STOP command.

safety_controller/launch/testing.launch.py

@@ -0,0 +1,39 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    safety_params = os.path.join(
+        get_package_share_directory('safety_controller'),
+        'params.yaml'
+    )
+    return LaunchDescription([
+        Node(
+            package='safety_controller',
+            executable='tester',
+            name='safety_tester',
+            parameters=[
+                {'drive_topic': '/vesc/high_level/input/nav_1'},
+            ]
+        ),
+        Node(
+            package='safety_controller',
+            executable='controller',
+            name='safety_controller',
+            parameters=[
+                safety_params,
+                {'scan_topic': '/scan'},
+                {'drive_topic_listen': '/vesc/low_level/ackermann_cmd'},
+                {'drive_topic_publish':'/vesc/low_level/input/safety'},
+            ]
+        ),
+        Node(
+            package='safety_controller',
+            executable='logger',
+            name='error_logger',
+            parameters=[
+
+            ]
+        )
+    ])

safety_controller/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>safety_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="racecar@todo.todo">racecar</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>ackermann_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+    <depend>ackermann_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>rclpy</depend>
+    <depend>sensor_msgs</depend>
+    <depend>std_msgs</depend>
+    <depend>visualization_msgs</depend>
+    <depend>tf2_ros</depend>
+  </export>
+</package>

safety_controller/safety_controller/logger_node.py

@@ -0,0 +1,89 @@
+import rclpy
+import random
+from rclpy.node import Node
+
+from std_msgs.msg import Float32
+from sensor_msgs.msg import LaserScan
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+class WallFollowerLogger(Node):
+# TODO: error metric: where we want it to stop - where it actually stopped
+# TODO: publish error metric WRT to different velocities
+
+    def __init__(self):
+        super().__init__('')
+        self.declare_parameters(namespace = '', parameters = [
+            ('subscriber_topic', 'crash_points'),
+        ])
+
+        self.subscription = self.create_subscription(
+            LaserScan,
+            self.get_parameter('subscriber_topic').value,
+            self.listener_callback,
+            10)
+
+        rclpy.get_shutdown_context().on_shutdown(self.on_shutdown_callback)
+
+        self.obstacle_sizes = []
+        self.avg_distances = []
+
+    def on_shutdown_callback(self):
+        self.get_logger().info("--- Node is shutting down. Saving a log of the run. ---")
+        obstacle_time_axis = [0 + i for i in range(len(self.obstacle_sizes))]
+        distance_time_axis = [0 + i for i in range(len(self.avg_distances))]
+
+        fig1 = plt.figure()
+        plt.plot(obstacle_time_axis,self.obstacle_sizes)
+        plt.figure('Obstacle Sizes Over Time During the Run')
+        plt.ylabel('Count of lidar points cluster')
+        plt.xlable('Timestep')
+
+        fig2 = plt.figure()
+        plt.plot(distance_time_axis,self.avg_distances)
+        plt.figure('Average Distance To Obstacles During the Run')
+        plt.ylabel('Distance (m)')
+        plt.xlable('Timestep')
+
+        fig1.savefig('obstacle_count')
+        fig2.savefig('avg_dist')
+        fig1.close()
+        fig2.close()
+
+        obstacles_df = pd.DataFrame(self.obstacle_sizes)
+        distances_df = pd.DataFrame(self.avg_distances)
+        obstacles_df.to_csv('obstacle_count',index=False)
+        distances_df.to_csv('avg_dist',index=False)
+        self.get_logger().info(f'Saved csv files and plots to {os.getcwd()}/')
+
+    def listener_callback(self, msg):
+        points = msg.ranges
+
+        n = len(points)
+        if n > 0:
+            self.get_logger().info(f'Currently detecting a crash of size: {n}')
+
+        avg_distance = np.mean(np.array(n))
+
+        self.obstacle_sizes.append(n)
+        self.avg_distances.append(avg_distance)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = WallFollowerLogger()
+
+    rclpy.spin(minimal_publisher)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

safety_controller/safety_controller/params.yaml

@@ -0,0 +1,4 @@
+safety_controller:
+  ros__parameters:
+    safety_radius: 0.5
+    safety_controller_const: 0.1