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

@@ -5,6 +5,9 @@ FROM ${BASE_IMAGE} AS source
 
 WORKDIR ${AMENT_WS}/src
 
+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
+
 # Clean up and update apt-get, then update rosdep
 RUN sudo apt-get clean && \
     sudo apt-get update && \
@@ -16,6 +19,10 @@ RUN sudo apt-get clean && \
 COPY src/robot .
 
 # 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' \
@@ -26,6 +33,10 @@ RUN apt-get -qq update && rosdep update && \
 FROM ${BASE_IMAGE} AS dependencies
 
 # Clean up and update apt-get, then update rosdep
+
+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 sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update
@@ -35,6 +46,7 @@ RUN sudo apt-get install libeigen3-dev
 
 # 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 +62,10 @@ 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 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 sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update
@@ -63,6 +79,29 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
 # Source and Build Artifact Cleanup 
 RUN rm -rf src/* build/* devel/* install/* log/*
 
+#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 pip3 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
+
+
 # Entrypoint will run before any CMD on launch. Sources ~/opt/<ROS_DISTRO>/setup.bash and ~/ament_ws/install/setup.bash
 COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh
 ENTRYPOINT ["./wato_ros_entrypoint.sh"]

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 \

docker/vis_tools/foxglove.Dockerfile

@@ -1,5 +1,6 @@
 ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04
 
+
 ################################ Source ################################
 FROM ${BASE_IMAGE} AS source
 
@@ -8,6 +9,9 @@ WORKDIR ${AMENT_WS}/src
 # Copy in source code 
 COPY src/wato_msgs wato_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 \
@@ -18,6 +22,9 @@ RUN apt-get -qq update && rosdep update && \
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} AS dependencies
 
+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
+
 # Install Foxglove Deps
 RUN apt-get update && apt-get install -y curl ros-humble-ros2bag ros-humble-rosbag2* ros-humble-foxglove-msgs&& \
     rm -rf /var/lib/apt/lists/*