Mapping and localization

.gitignore

@@ -3,3 +3,4 @@ __pycache__
 .vscode/
 
 watod-config.local.sh
+/log

config/ekf_testing.json

@@ -0,0 +1,115 @@
+{
+  "configById": {
+    "RawMessages!1fqah6": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/ekf/odom",
+      "fontSize": 12
+    },
+    "Plot!18f3n3x": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/autodrive/f1tenth_1/ips.x",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "Ground Truth X"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/wheel_odom.pose.pose.position.x",
+          "enabled": true,
+          "color": "#f5774d"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/ekf/odom.pose.pose.position.x",
+          "enabled": true,
+          "color": "#80f772"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
+    },
+    "RawMessages!13gpg4b": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/autodrive/f1tenth_1/ips",
+      "fontSize": 12
+    },
+    "Plot!2qhv9dy": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/autodrive/f1tenth_1/ips.y",
+          "enabled": true,
+          "color": "#4e98e2",
+          "label": "Ground truth Y"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/wheel_odom.pose.pose.position.y",
+          "enabled": true,
+          "color": "#f5774d"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/ekf/odom.pose.pose.position.y",
+          "enabled": true,
+          "color": "#80f772"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
+    }
+  },
+  "globalVariables": {},
+  "userNodes": {
+    "cec2ab24-3fb7-4faf-b079-3ff35cf0d557": {
+      "sourceCode": "import { Input, Message } from \"./types\";\n\ntype Output = {\n  data: boolean; // Qualification status\n};\n\n// Input topics\nexport const inputs = [\n  \"/autodrive/f1tenth_1/lap_count\",\n  \"/autodrive/f1tenth_1/collision_count\",\n];\n\n// Output topic\nexport const output = \"/autodrive/f1tenth_1/qualification_status\";\n\n// Variables\nlet lapCount = 0;\nlet collisionCount = 0;\nlet collisionsDuringWarmup = 0; // Collisions during warmup lap (0)\nlet collisionsDuringRace = 0; // Collisions during race laps (1-10)\n\n// Script\nexport default function script(\n  event:\n    | Input<\"/autodrive/f1tenth_1/lap_count\">\n    | Input<\"/autodrive/f1tenth_1/collision_count\">,\n): Output {\n  if (event.topic === \"/autodrive/f1tenth_1/lap_count\") {\n    // Read event.message fields expected for /autodrive/f1tenth_1/lap_count messages\n    lapCount = event.message.data;\n  } else {\n    // Read event.message fields expected for /autodrive/f1tenth_1/collision_count messages\n    collisionCount = event.message.data;\n  }\n\n  // Debugging\n  // log(event.message.data);\n\n  // Define the qualification status logic\n  let qualificationStatus = true;\n\n  if (lapCount < 1) {\n    // Ignore collisions during warmup lap (0)\n    collisionsDuringRace = 0;\n    collisionsDuringWarmup = collisionCount;\n    qualificationStatus = true;\n  } else if (lapCount >= 1 && lapCount <= 10) {\n    // Calculate collisions during race laps (1 to 10)\n    collisionsDuringRace = collisionCount - collisionsDuringWarmup;\n    qualificationStatus = collisionsDuringRace <= 10;\n  }\n\n  // Debugging\n  // log(collisionsDuringWarmup);\n  // log(collisionsDuringRace);\n  // log(qualificationStatus);\n\n  // Return the qualification status as the output\n  return {\n    data: qualificationStatus,\n  };\n}\n",
+      "name": "Qualification Status"
+    },
+    "a5eb1ac9-8dba-4b05-8e9f-a0c3ecdc3953": {
+      "sourceCode": "import { Input, Message } from \"./types\";\n\ntype Output = {\n  data: number; // Race time (seconds)\n};\n\n// Input topics\nexport const inputs = [\n  \"/autodrive/f1tenth_1/lap_count\",\n  \"/autodrive/f1tenth_1/last_lap_time\",\n];\n\n// Output topic\nexport const output = \"/autodrive/f1tenth_1/race_time\";\n\n// Variables\nlet lapCount = 0;\nlet lastLapCount = -1; // Track the previous lap count\nlet lastLapTime = 0; // Track the previous lap time\nlet raceTime = 0; // Time elapsed during race laps (1-10)\n\n// Script\nexport default function script(\n  event:\n    | Input<\"/autodrive/f1tenth_1/lap_count\">\n    | Input<\"/autodrive/f1tenth_1/last_lap_time\">,\n): Output {\n  if (event.topic === \"/autodrive/f1tenth_1/lap_count\") {\n    // Read event.message fields expected for /autodrive/f1tenth_1/lap_count messages\n    lapCount = event.message.data;\n  } else {\n    // Read event.message fields expected for /autodrive/f1tenth_1/last_lap_time messages\n    lastLapTime = event.message.data;\n  }\n\n  // Debugging\n  // log(event.message.data);\n\n  // Check time only for race laps\n  if (lapCount >= 3 && lapCount <= 12) {\n    // Only add time once per lap increment\n    if (lapCount > lastLapCount) {\n      raceTime += lastLapTime;\n      lastLapCount = lapCount;\n    }\n  }\n\n  // Debugging\n  // log(lastLapTime);\n  // log(raceTime);\n\n  // Return the total race time as the output\n  return {\n    data: raceTime,\n  };\n}\n",
+      "name": "Race Time"
+    },
+    "125add07-11e4-46e5-8c4e-84954294e443": {
+      "sourceCode": "import { Input, Message } from \"./types\";\n\ntype Output = {\n  data: number; // Adjusted race time (seconds)\n};\n\n// Input topics\nexport const inputs = [\n  \"/autodrive/f1tenth_1/lap_count\",\n  \"/autodrive/f1tenth_1/collision_count\",\n  \"/autodrive/f1tenth_1/last_lap_time\",\n];\n\n// Output topic\nexport const output = \"/autodrive/f1tenth_1/adjusted_race_time\";\n\n// Variables\nlet lapCount = 0; // Current lap count\nlet collisionCount = 0; // Current collision count\nlet collisionsDuringWarmup = 0; // Collisions during warmup lap (0)\nlet collisionsDuringRace = 0; // Collisions during race laps (1-10)\nlet lastLapCount = -1; // Track the previous lap count\nlet lastLapTime = 0; // Track the previous lap time\nlet raceTime = 0; // Time elapsed during race laps (1-10)\nlet adjustedRaceTime = 0; // Race time with penalties\n\n// Script\nexport default function script(\n  event:\n    | Input<\"/autodrive/f1tenth_1/lap_count\">\n    | Input<\"/autodrive/f1tenth_1/collision_count\">\n    | Input<\"/autodrive/f1tenth_1/last_lap_time\">,\n): Output {\n  if (event.topic === \"/autodrive/f1tenth_1/lap_count\") {\n    // Read event.message fields expected for /autodrive/f1tenth_1/lap_count messages\n    lapCount = event.message.data;\n  } else if (event.topic === \"/autodrive/f1tenth_1/collision_count\") {\n    // Read event.message fields expected for /autodrive/f1tenth_1/collision_count messages\n    collisionCount = event.message.data;\n  } else {\n    // Read event.message fields expected for /autodrive/f1tenth_1/last_lap_time messages\n    lastLapTime = event.message.data;\n  }\n\n  // Debugging\n  // log(event.message.data);\n\n  // Calculate collisions only for race laps\n  if (lapCount < 1) {\n    // Ignore collisions during warmup lap (0)\n    collisionsDuringRace = 0;\n    collisionsDuringWarmup = collisionCount;\n  } else if (lapCount >= 1 && lapCount <= 10) {\n    // Calculate collisions during race laps (1 to 10)\n    collisionsDuringRace = collisionCount - collisionsDuringWarmup;\n  }\n\n  // Check time only for race laps\n  if (lapCount >= 3 && lapCount <= 12) {\n    // Only add time once per lap increment\n    if (lapCount > lastLapCount) {\n      raceTime += lastLapTime;\n      lastLapCount = lapCount;\n    }\n  }\n\n  // Calculate adjusted race time\n  if (lapCount > 11) {\n    adjustedRaceTime = raceTime + 10 * collisionsDuringRace;\n  }\n\n  // Debugging\n  // log(collisionsDuringWarmup);\n  // log(collisionsDuringRace);\n  // log(lastLapTime);\n  // log(raceTime);\n  // log(adjustedRaceTime);\n\n  // Return the qualification status as the output\n  return {\n    data: adjustedRaceTime,\n  };\n}\n",
+      "name": "Adjusted Race Time"
+    }
+  },
+  "playbackConfig": {
+    "speed": 1
+  },
+  "layout": {
+    "first": {
+      "first": "RawMessages!1fqah6",
+      "second": "Plot!18f3n3x",
+      "direction": "column",
+      "splitPercentage": 40.728476821192054
+    },
+    "second": {
+      "first": "RawMessages!13gpg4b",
+      "second": "Plot!2qhv9dy",
+      "direction": "column",
+      "splitPercentage": 40.728476821192054
+    },
+    "direction": "row"
+  }
+}

docker/robot/robot.Dockerfile

@@ -10,12 +10,16 @@ RUN sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update
 
+RUN sudo apt install -y python3
+RUN sudo apt install -y python3-pip
+
 # Copy in source code
 # COPY src/robot/bringup_robot bringup_robot
 # COPY src/robot/gym_vis gym_vis
 COPY src/robot .
 
 # Scan for rosdeps
+
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \
         | grep 'apt-get install' \
@@ -26,15 +30,39 @@ RUN apt-get -qq update && rosdep update && \
 FROM ${BASE_IMAGE} AS dependencies
 
 # Clean up and update apt-get, then update rosdep
+
 RUN sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update
 
 # ADD MORE DEPENDENCIES HERE
 RUN sudo apt-get install libeigen3-dev
 
+#add the slam toolbox, localizaiton and rviz2
+RUN sudo apt-get update
+RUN sudo apt-get install -y ros-humble-rviz2
+RUN sudo apt-get install -y ros-humble-navigation2
+RUN sudo apt-get install -y ros-humble-slam-toolbox
+
+#add controler support to the container
+RUN sudo apt-get install -y ros-humble-joy 
+RUN sudo apt-get install -y jstest-gtk
+RUN mkdir -p /root/.config/jstest-gtk
+
+#add the nesscary depends for particle filter
+RUN sudo apt-get install -y pip
+RUN sudo pip install cython
+
+RUN mkdir -p /tmp/build && \
+    cd /tmp/build && \
+    git clone https://github.com/f1tenth/range_libc.git && \
+    cd range_libc/pywrapper && \
+    python3 setup.py install && \
+    cd / && rm -rf /tmp/build
+
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
+RUN apt-get update
 RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
 
 # Copy in source code from source stage
@@ -50,6 +78,7 @@ RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \
 FROM dependencies AS build
 
 # Clean up and update apt-get, then update rosdep
+
 RUN sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update

docker/samples/cpp_aggregator.Dockerfile

@@ -10,6 +10,10 @@ COPY src/samples/cpp/aggregator aggregator
 COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
+
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \
         | grep 'apt-get install' \

docker/samples/cpp_producer.Dockerfile

@@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src
 COPY src/samples/cpp/producer producer
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

docker/samples/cpp_transformer.Dockerfile

@@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src
 COPY src/samples/cpp/transformer transformer
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

docker/samples/py_aggregator.Dockerfile

@@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src
 COPY src/samples/python/aggregator aggregator
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

docker/samples/py_producer.Dockerfile

@@ -1,5 +1,6 @@
 ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04
 
+
 ################################ Source ################################
 FROM ${BASE_IMAGE} AS source
 
@@ -9,6 +10,9 @@ WORKDIR ${AMENT_WS}/src
 COPY src/samples/python/producer producer
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

docker/samples/py_transformer.Dockerfile

@@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src
 COPY src/samples/python/transformer transformer
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+RUN apt-get update && apt-get install -y curl \
+ && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

linux.setup.md

@@ -0,0 +1,100 @@
+# WATonomous F1Tenth
+
+## linux Setup for AutoDRIVE F1Tenth Simulator
+
+to get setup and start developing on the AutoDRIVE sim, you will need to following do the following: 
+1. Have Docker Installed on the host system
+2. Have the Nvidea container toolkit installed
+3. setup the wato_f1teenth workspace
+4. Download the Autodrive sim 
+
+Additionally, I can only guarantee that this works on on the following linux distros, as these are only ones tested : 
+1. Ubuntu 24.04 (LTS)
+2. Ubuntu 22.04 (LTS)
+3. Fedora 42 
+
+you can try to install or follow the guide if you are on other distros, but support can't be guarantee
+
+## Installing Docker
+
+Installing Docker is pretty intuitive, However it is not recomended that you install it using your package manager such as apt or dnf. its is recomended that your go the docker website and follow the instruction for your own distro 
+
+[link to docker install page](https://docs.docker.com/engine/install/)
+
+
+## Installing the Nvidia Container toolkit 
+
+If you have an Nvidia gpu it is highly recomended that you install the Nvidia Container toolkit, as some of the algorithms need it to run properly such as the current implementation of our particle filter. 
+
+before you install the Nvidia Container toolkit, ensure that your Nvidia drivers are installed correctly. you can run the following comand to figure it out: 
+```
+nvidia-smi
+```
+if the drivers are installed properly, you should see an output simmilar to this with your gpu: 
+![alt text](config/images/image.png)
+
+if you don't see something like this, that means that your drivers are not installed properly, and you need to go install them. 
+
+you can use the following docs provided by Nvidia or any yt tutorial of your choice to get the job done. [link to driver install](https://docs.nvidia.com/datacenter/tesla/driver-installation-guide/index.html) 
+
+Given you have installed your driver and can see an output similar to the one I got you can go do the following if you are on an ubuntu based system: 
+
+1. Configure the production repository: 
+```
+curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
+  && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \
+    sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
+    sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
+```
+
+2. Update the packages list from the repository : 
+```
+sudo apt-get update
+```
+3. Install the NVIDIA Container Toolkit packages
+```
+export NVIDIA_CONTAINER_TOOLKIT_VERSION=1.17.8-1
+  sudo apt-get install -y \
+      nvidia-container-toolkit=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \
+      nvidia-container-toolkit-base=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \
+      libnvidia-container-tools=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \
+      libnvidia-container1=${NVIDIA_CONTAINER_TOOLKIT_VERSION}
+```
+5. Configure the container runtime by using the ```nvidia-ctk``` command
+```
+sudo nvidia-ctk runtime configure --runtime=docker
+```
+6. Restart the Docker daemon using the following or by restarting your system 
+```
+sudo systemctl restart docker
+```
+
+if you were not on an ubuntu based system, the first 3 comands can varray, so go the the actual [docs](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html).
+
+to check that everything is working properly, lauch up the container using ```./watod build && ./watod up``` and run the ```nvidia-smi``` inside of the container. you should get a similar output like before: 
+![alt text](config/images/image2.png)
+
+## Setup the Wato F1teenth Workspace
+
+1. In your home directory (~), run: `mkdir ros_ws && cd ros_ws`, then run
+`git clone https://github.com/WATonomous/wato_f1tenth.git`
+2. Go into the repo: `cd wato_f1tenth`
+3. Start VSCode: `code .`
+4. Make sure the `watod-config.sh` file has: `ACTIVE_MODULES="vis_tools robot sim"` and `MODE_OF_OPERATION="develop"`. 
+5. Run `./watod build && ./watod up` to start the containers. Future starts can just be `./watod up` if you don't need to rebuild containers. **(if you would like to run gui apps in the container, run `xhost + local:root` before starting the container)**
+6. Download the Docker extension for VSCode. 
+7. From the extension pannel, right click on the `-robot_dev-1` container and attach a VSCode.
+8. In the opened VSCode window select the `/home/bolty/ament_ws` folder. This will be where you do your development!
+
+## Download the Autodrive sim 
+
+1. Download the `practice` simulator under Local Resources from: https://autodrive-ecosystem.github.io/competitions/f1tenth-sim-racing-iros-2024/#resources
+2. Unzip the download and move the `autodrive_simulator` to the ros_ws
+3. then `cd autodrive_simulator` and rename the file `AutoDRIVE Simulator.x86_64` to `sim.x86_64`
+4. run `chmod + sim.x86_64` to make it excutable then run `./sim.x86_64` to open the sim
+5. Press on the button "Disconnected" to attempt connection to your running watod containers. The default port should already be 4567.
+6. You should see "Connected"- this means your physics simulator is connected to your watod containers and everything is setup correctly!
+![alt text](config/images/image3.png)
+![alt text](config/images/image4.png)
+7. 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.
+