finished onboarding

docker/robot/robot.Dockerfile

@@ -3,52 +3,86 @@ ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04
 ################################ Source ################################
 FROM ${BASE_IMAGE} AS source
 
+RUN mkdir -p /usr/share/keyrings && \
+    rm -f /etc/apt/sources.list.d/ros2-latest.list && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | \
+    gpg --batch --yes --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \
+    > /etc/apt/sources.list.d/ros2.list
+
 WORKDIR ${AMENT_WS}/src
 
 # Clean up and update apt-get, then update rosdep
 RUN sudo apt-get clean && \
     sudo apt-get update && \
     sudo rosdep update
 
-# 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' \
-        | awk '{print $3}' \
-        | sort  > /tmp/colcon_install_list
+    | grep 'apt-get install' \
+    | awk '{print $3}' \
+    | sort  > /tmp/colcon_install_list
 
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} AS dependencies
 
+# Remove conflicting ROS 2 APT source and update GPG key
+RUN rm -f /etc/apt/sources.list.d/ros2-latest.list && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | \
+    gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \
+    > /etc/apt/sources.list.d/ros2.list
+
 # 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 rm -rf /var/lib/apt/lists/* && \
+    apt-get update && \
+    apt-get install -y --no-install-recommends \
+    curl \
+    ros-humble-ros2bag \
+    ros-humble-rosbag2 \
+    ros-humble-rosbag2-storage \
+    ros-humble-rosbag2-transport \
+    ros-humble-rosbag2-compression \
+    ros-humble-rosbag2-interfaces \
+    ros-humble-rosbag2-storage-default-plugins \
+    ros-humble-foxglove-msgs \
+    && rm -rf /var/lib/apt/lists/*
+
 RUN sudo apt-get install libeigen3-dev
 
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
-RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends $(cat /tmp/colcon_install_list) || \
+    apt-get install -y --no-install-recommends --fix-missing
 
 # Copy in source code from source stage
 WORKDIR ${AMENT_WS}
 COPY --from=source ${AMENT_WS}/src src
 
 # Dependency Cleanup
-WORKDIR / 
+WORKDIR /
 RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \
     rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/*
 
 ################################ Build ################################
 FROM dependencies AS build
 
+RUN mkdir -p /usr/share/keyrings && \
+    rm -f /etc/apt/sources.list.d/ros2-latest.list && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | \
+    gpg --batch --yes --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \
+    > /etc/apt/sources.list.d/ros2.list
+
 # Clean up and update apt-get, then update rosdep
 RUN sudo apt-get clean && \
     sudo apt-get update && \
@@ -58,11 +92,10 @@ RUN sudo apt-get clean && \
 WORKDIR ${AMENT_WS}
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
     colcon build \
-        --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base ${WATONOMOUS_INSTALL}
+    --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base ${WATONOMOUS_INSTALL}
 
-# Source and Build Artifact Cleanup 
+# Source and Build Artifact Cleanup
 RUN rm -rf src/* build/* devel/* install/* log/*
 
-# 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/vis_tools/foxglove.Dockerfile

@@ -11,50 +11,58 @@ COPY src/wato_msgs wato_msgs
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \
-        | grep 'apt-get install' \
-        | awk '{print $3}' \
-        | sort  > /tmp/colcon_install_list
+    | grep 'apt-get install' \
+    | awk '{print $3}' \
+    | sort  > /tmp/colcon_install_list
 
 ################################# Dependencies ################################
+# ...existing code...
+
 FROM ${BASE_IMAGE} AS dependencies
 
-# 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/*
+# Remove conflicting ROS 2 APT source and update GPG key
+RUN rm -f /etc/apt/sources.list.d/ros2*.list && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | \
+    gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
+    > /etc/apt/sources.list.d/ros2.list
 
-# Set up apt repo
-RUN apt-get update && apt-get install -y lsb-release software-properties-common apt-transport-https && \
-    apt-add-repository universe
+# Clean up and update apt-get, then update rosdep
+RUN apt-get clean && \
+    apt-get update && \
+    rosdep update
 
-# Install Dependencies
-RUN apt-get update && \
-    apt-get install -y \ 
-    ros-$ROS_DISTRO-foxglove-bridge \
-    ros-$ROS_DISTRO-rosbridge-server \
-    ros-$ROS_DISTRO-topic-tools \
-    ros-$ROS_DISTRO-vision-msgs
+# Install Foxglove Deps
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    curl \
+    ros-humble-ros2bag || true && \
+    apt-get install -y --no-install-recommends \
+    ros-humble-rosbag2-storage-default-plugins || true && \
+    apt-get install -y --no-install-recommends \
+    ros-humble-rosbag2-compression || true && \
+    apt-get install -y --no-install-recommends \
+    ros-humble-rosbag2-transport || true && \
+    apt-get install -y --no-install-recommends \
+    ros-humble-foxglove-msgs || true && \
+    rm -rf /var/lib/apt/lists/*
 
-# Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
 RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
 
-# Copy in source code from source stage
 WORKDIR ${AMENT_WS}
 COPY --from=source ${AMENT_WS}/src src
 
-# Dependency Cleanup
 WORKDIR /
 RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \
     rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/*
 
-################################ Build ################################
 FROM dependencies AS build
 
 # Build ROS2 packages
 WORKDIR ${AMENT_WS}
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
     colcon build \
-        --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base ${WATONOMOUS_INSTALL}
+    --cmake-args -DCMAKE_BUILD_TYPE=Release --install-base ${WATONOMOUS_INSTALL}
 
 # Source and Build Artifact Cleanup 
 RUN rm -rf src/* build/* devel/* install/* log/*

docker/wato_ros_entrypoint.sh

@@ -1,6 +1,11 @@
 #!/bin/bash
 set -e
 
-# setup ROS2 environment
 source /opt/watonomous/setup.bash
-exec "$@"
+source /opt/ros/humble/setup.bash
+
+cd /home/bolty/ament_ws
+colcon build --merge-install --symlink-install
+source install/setup.bash
+
+exec ros2 run imu_throttle imu_throttle

src/robot/imu_throttle/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(imu_throttle)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+add_executable(imu_throttle ./imu_throttle.cpp)
+target_include_directories(imu_throttle PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(imu_throttle rclcpp sensor_msgs geometry_msgs)
+
+install(TARGETS imu_throttle
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+  OPTIONAL)
+
+install(DIRECTORY
+  include/
+  DESTINATION include/
+  OPTIONAL)
+
+ament_package()

src/robot/imu_throttle/imu_throttle.cpp

@@ -0,0 +1,112 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_msgs/msg/float32.hpp>
+
+class ImuThrottle : public rclcpp::Node
+{
+public:
+    ImuThrottle();
+
+private:
+    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
+    void lidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr throttle_pub_;
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
+
+    bool stopped;
+    float throttle;
+};
+
+ImuThrottle::ImuThrottle()
+    : Node("imu_throttle"), stopped(false), throttle(-1.0f)
+{
+    RCLCPP_INFO(this->get_logger(), "IMU Throttle node started");
+
+    imu_pub_ = create_publisher<sensor_msgs::msg::Imu>("/autodrive/f1tenth_1/imu_throttle", 10);
+    throttle_pub_ = create_publisher<std_msgs::msg::Float32>("/autodrive/f1tenth_1/throttle_command", 10);
+
+    imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+        "/autodrive/f1tenth_1/imu", 10,
+        std::bind(&ImuThrottle::imuCallback, this, std::placeholders::_1));
+
+    lidar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+        "/autodrive/f1tenth_1/lidar", 10,
+        std::bind(&ImuThrottle::lidarCallback, this, std::placeholders::_1));
+}
+
+void ImuThrottle::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
+{
+    RCLCPP_INFO(this->get_logger(), "Received IMU: [%.3f, %.3f, %.3f]",
+                msg->angular_velocity.x,
+                msg->angular_velocity.y,
+                msg->angular_velocity.z);
+
+    if (msg->angular_velocity.x > 0.1 ||
+        msg->angular_velocity.y > 0.1 ||
+        msg->angular_velocity.z > 0.1)
+    {
+        RCLCPP_INFO(this->get_logger(), "Publishing filtered IMU");
+        imu_pub_->publish(*msg);
+    }
+}
+
+void ImuThrottle::lidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
+{
+    if (msg->ranges.empty())
+    {
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
+                             "empty ranges from LiDAR");
+        return;
+    }
+
+    int center_index = msg->ranges.size() / 2;
+    RCLCPP_INFO(this->get_logger(), "Front distance: %.2f", msg->ranges[center_index]);
+    float front_distance = msg->ranges[center_index];
+
+    if (std::isnan(front_distance) ||
+        front_distance < msg->range_min)
+    {
+        return;
+    }
+
+    constexpr float STOP_THRESHOLD = 2.0f;
+
+    float desired_throttle;
+
+    if (front_distance < STOP_THRESHOLD)
+    {
+        desired_throttle = 0.0f;
+        if (desired_throttle != throttle)
+        {
+            std_msgs::msg::Float32 throttle_msg;
+            throttle_msg.data = desired_throttle;
+            throttle_pub_->publish(throttle_msg);
+            throttle = desired_throttle;
+            RCLCPP_INFO(this->get_logger(), "Obstacle at %.2fm — setting throttle to 0.0", front_distance);
+        }
+        return;
+    }
+    desired_throttle = 1.0f;
+
+    RCLCPP_INFO(this->get_logger(), "Setting throttle to 1.0f");
+
+    if (desired_throttle != throttle)
+    {
+        std_msgs::msg::Float32 throttle_msg;
+        throttle_msg.data = desired_throttle;
+        throttle_pub_->publish(throttle_msg);
+        throttle = desired_throttle;
+    }
+}
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<ImuThrottle>());
+    rclcpp::shutdown();
+    return 0;
+}

src/robot/imu_throttle/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>imu_throttle</name>
+    <version>0.0.0</version>
+    <description>IMU package, prints to terminal and drives car</description>
+    <maintainer email="[email protected]">Shivam Walia</maintainer>
+    <license>Apache-2.0</license>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <build_depend>rclcpp</build_depend>
+    <build_depend>sensor_msgs</build_depend>
+    <build_depend>geometry_msgs</build_depend>
+    <exec_depend>rclcpp</exec_depend>
+    <exec_depend>sensor_msgs</exec_depend>
+    <exec_depend>geometry_msgs</exec_depend>
+</package>

src/wato_msgs/foxglove_msgs/package.xml

@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>std_msgs</depend>
-  <depend>ackermann_msgs</depend>
+  <depend>ros-humble-ackermann-msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

watod-config.sh

@@ -38,4 +38,4 @@ MODE_OF_OPERATION="develop"
 
 ## Platform in which to build the docker images with. 
 ## Either arm64 (apple silicon, raspberry pi) or amd64 (most computers)
-# PLATFORM="amd64"
+PLATFORM="arm64"