diff --git a/README.md b/README.md index 14f9986..6cc3247 100644 --- a/README.md +++ b/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! diff --git a/docker/robot/robot.Dockerfile b/docker/robot/robot.Dockerfile index 37b483d..99e65cd 100644 --- a/docker/robot/robot.Dockerfile +++ b/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 diff --git a/docker/sim/sim_autodrive.Dockerfile b/docker/sim/sim_autodrive.Dockerfile new file mode 100644 index 0000000..4cdad5e --- /dev/null +++ b/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"] \ No newline at end of file diff --git a/docker/sim/sim_autodrive_entrypoint.sh b/docker/sim/sim_autodrive_entrypoint.sh new file mode 100755 index 0000000..bd6a5e8 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/modules/docker-compose.sim.yaml b/modules/docker-compose.sim.yaml index 90cf81a..d5b1b88 100644 --- a/modules/docker-compose.sim.yaml +++ b/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 diff --git a/src/robot/emergency_braking/CMakeLists.txt b/src/robot/emergency_braking/CMakeLists.txt new file mode 100644 index 0000000..a000159 --- /dev/null +++ b/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( 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() diff --git a/src/robot/emergency_braking/package.xml b/src/robot/emergency_braking/package.xml new file mode 100644 index 0000000..1dc0af7 --- /dev/null +++ b/src/robot/emergency_braking/package.xml @@ -0,0 +1,18 @@ + + + + emergency_braking + 0.0.0 + TODO: Package description + bolty + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robot/emergency_braking/src/emergency_braking.cpp b/src/robot/emergency_braking/src/emergency_braking.cpp new file mode 100644 index 0000000..38ca9d2 --- /dev/null +++ b/src/robot/emergency_braking/src/emergency_braking.cpp @@ -0,0 +1,114 @@ +#include +#include +#include + +#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("drive", 10); + scan_sub_ = this->create_subscription( + "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::SharedPtr brake_pub_; + rclcpp::Subscription::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(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 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::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(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()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file