Emergency braking

README.md

@@ -1,15 +1,28 @@
 # WATonomous F1Tenth
 
-## Initial Windows Setup for visualizing in Foxglove
+## Windows Setup for AutoDRIVE F1Tenth Simulator
+
+#### Development Space Setup
 1. Download Ubuntu in Microsoft Store (The one without any extension)
 2. Install Docker Destop: Settings -> Resources -> WSL Integration -> Enable Ubuntu
 3. Go into Ubuntu and setup username and password, if first time installing
-4. In the your home directory (~), run: git clone https://github.com/WATonomous/wato_f1tenth.git
-5. Go into the repo by running: cd wato_f1tenth
-6. Attach vscode to this repo by running (Asssuming you have vscode): code . 
-7. In VS Code, manually forward the port by going to PORTS -> Add Port -> Enter 20000 for port number
-8. Run ./watod up to build and up the container
-9. Go to foxglove: Open Connection -> Foxglove Websocket -> Enter ws://localhost:20000
-10. Now go to the Panel section on the left -> click the 3D Panel in the middle -> Topics -> Hover your cursor on the map and click the eye button to make map visible
-
-Now you should be able to see the car with transforms and the map
+4. In your home directory (~), run: `git clone https://github.com/WATonomous/wato_f1tenth.git`
+5. Go into the repo: `cd wato_f1tenth`
+6. Start VSCode: `code .`
+7. Make sure the `watod-config.sh` file has: `ACTIVE_MODULES="vis_tools robot sim"` and `MODE_OF_OPERATION="develop"`. 
+8. Run `./watod build && ./watod up` to start the containers. Future starts can just be `./watod up` if you don't need to rebuild containers.
+9. Download the Docker extension for VSCode. 
+10. From the extension pannel, right click on the `-robot_dev-1` container and attach a VSCode.
+11. In the opened VSCode window select the `/home/bolty/ament_ws` folder. This will be where you do your development!
+
+#### Connecting the AutoDRIVE Simulator
+1. Download the `practice` simulator from: https://autodrive-ecosystem.github.io/competitions/f1tenth-sim-racing-iros-2024/#resources
+2. Unzip the download and run `AutoDRIVE Simulator.exe` inside the folder
+3. Open the left menu on the simulator application
+4. Press on the button "Disconnected" to attempt connection to your running watod containers. The default port should already be 4567.
+5. You should see "Connected"- this means your physics simulator is connected to your watod containers and everything is setup correctly!
+6. There is built-in Keyboard Teleop in the simulator. Click on "Autonomous" to set driving mode to "Manual", you will be able to drive with WASD.
+
+#### Note for other OS (Ubuntu and MacOS)
+This guide was only written & tested on Windows. 
+- Try the above steps and if you have any problems, ping an F1Tenth Lead on the Discord channel!

docker/robot/robot.Dockerfile

@@ -31,6 +31,7 @@ RUN sudo apt-get clean && \
     sudo rosdep update
 
 # ADD MORE DEPENDENCIES HERE
+RUN sudo apt-get install libeigen3-dev
 
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list

docker/sim/sim_autodrive.Dockerfile

@@ -0,0 +1,29 @@
+# MIT License
+
+# Copyright (c) 2020 Hongrui Zheng
+
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+FROM autodriveecosystem/autodrive_f1tenth_api:2024-cdc-practice
+
+SHELL ["/bin/bash", "-c"]
+
+WORKDIR /home/autodrive_devkit
+COPY docker/sim/sim_autodrive_entrypoint.sh /home/autodrive_devkit/sim_autodrive_entrypoint.sh
+ENTRYPOINT ["./sim_autodrive_entrypoint.sh"]

docker/sim/sim_autodrive_entrypoint.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+set -e
+
+# Launch API node
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+sed -i '284c\        autodrive.lidar_range_array = np.fromstring(data["V1 LIDAR Range Array"], sep="\\n")' src/autodrive_devkit/autodrive_f1tenth/autodrive_bridge.py
+colcon build
+ros2 launch autodrive_f1tenth simulator_bringup_headless.launch.py

modules/docker-compose.sim.yaml

@@ -2,17 +2,21 @@ services:
   sim:
     build:
       context: ..
-      dockerfile: docker/sim/sim.Dockerfile
+      dockerfile: docker/sim/sim_autodrive.Dockerfile
       # cache_from:
       #   - "${GAZEBO_SERVER_IMAGE:?}:${TAG}"
       #   - "${GAZEBO_SERVER_IMAGE:?}:main"
       # args:
       #   BASE_IMAGE: ${BASE_IMAGE_OVERRIDE-}
     # image: "${GAZEBO_SERVER_IMAGE:?}:${TAG}"
-    image: f1tenth:latest
-    # ports:
-    #   - "${GAZEBO_PORT:?}:9002"
+    image: autodrive_f1tenth:latest
+    ports:
+      - "4567:4567"
+      - "7400:7400/udp"
+      - "7401:7401/udp"
+      - "7410:7410/udp"
+      - "7411:7411/udp"
     profiles: [deploy, develop]
-    command: /bin/bash -c "ros2 launch f1tenth_gym_ros gym_bridge_launch.py"
+    # command: /bin/sh -c "echosus" #"ros2 launch autodrive_f1tenth simulator_bringup_headless.launch.py"
     # tty: true
     # command: /bin/bash

src/robot/emergency_braking/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.8)
+project(emergency_braking)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
+
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+add_executable(emergency_braking_node src/emergency_braking.cpp)
+target_include_directories(emergency_braking_node PUBLIC include)
+
+
+
+ament_target_dependencies(emergency_braking_node rclcpp std_msgs nav_msgs visualization_msgs sensor_msgs ackermann_msgs tf2_ros tf2_geometry_msgs)
+
+
+# Install the meshes folder
+#install(DIRECTORY assets/
+#  DESTINATION share/${PROJECT_NAME}/assets
+#)
+
+install(TARGETS
+  emergency_braking_node
+
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+ament_package()

src/robot/emergency_braking/package.xml

@@ -0,0 +1,18 @@
+<?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>emergency_braking</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">bolty</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robot/emergency_braking/src/emergency_braking.cpp

@@ -0,0 +1,114 @@
+#include <cmath>
+#include <vector>
+#include <limits>
+
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+using std::placeholders::_1;
+
+class EmergencyBraking : public rclcpp::Node {
+
+   public:
+        EmergencyBraking() : Node("emergency_braking_node") {
+            // publisher and subscriber
+            brake_pub_ = this->create_publisher<ackermann_msgs::msg::AckermannDriveStamped>("drive", 10);
+            scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+                "scan", 10, std::bind(&EmergencyBraking::scan_callback, this, _1));
+        }
+
+   private:
+        // constants
+        static constexpr double threshold = 0.05; 
+        static constexpr bool brake_default = false;
+        static constexpr int window_size = 11; // how many lidar beams to use in forward direction
+        static constexpr double min_speed = 0.1; // smallest speed to consider for calculations
+
+        rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr brake_pub_;
+        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+
+        // lidar scan callback
+        void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg) {
+            // index (idx) of beam at angle 0 (directly forward)
+            // static_cast to convert double to int
+            int forward_idx = static_cast<int>(std::round((0.0 - scan_msg->angle_min) / scan_msg->angle_increment));
+            int half_window = window_size / 2;
+
+            // Store previous ranges for each beam in the window to estimate speed
+            // initialize each element to 0.0
+            // window_size is number of elements
+            static std::vector<double> prev_ranges(window_size, 0.0);
+            // Store previous timestamp to compute time difference
+            static rclcpp::Time prev_time = scan_msg->header.stamp;
+            rclcpp::Time curr_time = scan_msg->header.stamp;
+            // dt: change in time
+            double dt = (curr_time - prev_time).seconds();
+            prev_time = curr_time;
+
+            // Minimum TTC found in window
+            // set the initial value to the maximum 
+            double min_ttc = std::numeric_limits<double>::max(); 
+
+            // Number of valid beams used
+            // start at 0 since no beams were used yet
+            int valid_count = 0; 
+
+            // iterate through the window of beams aroinund the forward direction
+            for (int i = -half_window; i <= half_window; ++i) {
+                int idx = forward_idx + i;
+                // exit the loop if the index is negative or too large
+                if (idx < 0 || idx >= static_cast<int>(scan_msg->ranges.size())) continue;
+                // get lidar distance measurement at index idx
+                double range = scan_msg->ranges[idx];
+                // exit the loop if the index is invalid
+                if (std::isnan(range) || range > scan_msg->range_max || range < scan_msg->range_min) continue;
+
+                // previous index
+                int prev_idx = i + half_window;
+                if (prev_ranges[prev_idx] == 0.0) prev_ranges[prev_idx] = range;
+
+                double speed = 0.0;
+                // Estimate speed using distance and time
+                if (dt > 0.0) {
+                    speed = (prev_ranges[prev_idx] - range) / dt;
+                }
+                prev_ranges[prev_idx] = range;
+
+                // exit loop if speed is less than min speed
+                if (speed <= min_speed) continue;
+
+                double ttc = range / speed;
+                if (ttc < min_ttc) min_ttc = ttc;
+
+                valid_count++;
+            }
+
+            // If no valid beams, skip braking logic
+            if (valid_count == 0) {
+                RCLCPP_INFO(this->get_logger(), "No valid beams for TTC calculation.");
+                return;
+            }
+
+            bool brake = brake_default;
+            RCLCPP_INFO(this->get_logger(), "min_ttc: %f", min_ttc);
+
+            // Brake if the minimum TTC is below the threshold
+            if (min_ttc < threshold) {
+                brake = true;
+            }
+            if (brake) {
+                auto brake_msg = ackermann_msgs::msg::AckermannDriveStamped();
+                brake_msg.drive.speed = 0.0;
+                RCLCPP_INFO(this->get_logger(), "stop car");
+                this->brake_pub_->publish(brake_msg);
+            }
+        }
+};
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<EmergencyBraking>());
+    rclcpp::shutdown();
+    return 0;
+}