Sarah Yoo Onboarding

.gitignore

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

README.md

@@ -1,12 +1,28 @@
-# WATonomous ASD Admissions Assignment
+## Project Demo
+This video demonstrates the current prototype of our autonomous robot, showcasing LiDAR-based SLAM mapping and path planning in action.  
+
+[![Watch the Demo](https://img.youtube.com/vi/wIKEoOrXFpg/0.jpg)](https://www.youtube.com/watch?v=wIKEoOrXFpg)  
+*Click the image to watch the full demo on YouTube.*
 
 ## Prerequisite Installation
-These steps are to setup the monorepo to work on your own PC. We utilize docker to enable ease of reproducibility and deployability.
 
-> Why docker? It's so that you don't need to download any coding libraries on your bare metal pc, saving headache :3
+These steps will set up the monorepo on your local machine. We use **Docker** to ensure reproducibility and simplify deployment.  
+
+1. **Operating System Support:**  
+   This project supports **Linux (Ubuntu ≥ 22.04), Windows (via WSL), and macOS**. You can set up your environment using one of the following approaches:  
+   - [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview)  
+   - [Windows Subsystem for Linux (WSL)](https://learn.microsoft.com/en-us/windows/wsl/install)  
+   - [Dual Boot Linux Setup](https://opensource.com/article/18/5/dual-boot-linux)  
 
-1. This assignment is supported on Linux Ubuntu >= 22.04, Windows (WSL), and MacOS. This is standard practice that roboticists can't get around. To setup, you can either setup an [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview), setting up [WSL](https://learn.microsoft.com/en-us/windows/wsl/install), or setting up your computer to [dual boot](https://opensource.com/article/18/5/dual-boot-linux). You can find online resources for all three approaches.
-2. Once inside Linux, [Download Docker Engine using the `apt` repository](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository)
-3. You're all set! You can begin the assignment by visiting the WATonomous Wiki.
+2. **Install Docker:**  
+   Once your Linux environment is ready, follow the [official Docker installation guide](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository) to install Docker Engine.  
 
-Link to Onboarding Assignment: https://wiki.watonomous.ca/
+3. **Build and Run the Project:**  
+   - Build the project:  
+     ```bash
+     ./watod build
+     ```  
+   - Start the simulation locally on Foxglobe:  
+     ```bash
+     ./watod up
+     ```  

frames_2026-01-09_00.35.06.gv

@@ -0,0 +1,14 @@
+digraph G {
+"robot" -> "robot/right_wheel"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"sim_world" -> "robot"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot" -> "robot/left_wheel"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"robot" -> "robot/caster"[label=" Broadcaster: default_authority\nAverage rate: 1001.024\nBuffer length: 0.977\nMost recent transform: 94.638\nOldest transform: 93.661\n"];
+"robot/chassis" -> "robot/chassis/camera"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot" -> "robot/chassis"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot/chassis" -> "robot/chassis/lidar"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+"robot/chassis" -> "robot/chassis/imu_sensor"[label=" Broadcaster: default_authority\nAverage rate: 1001.029\nBuffer length: 0.972\nMost recent transform: 94.638\nOldest transform: 93.666\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1767936906.2349072"[ shape=plaintext ] ;
+}->"sim_world";
+}

install/.colcon_install_layout

@@ -0,0 +1 @@
+isolated

log/latest

@@ -0,0 +1 @@
+latest_build

log/latest_build

@@ -0,0 +1 @@
+build_2026-01-08_08-26-27

src/robot/control/CMakeLists.txt

@@ -13,6 +13,8 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -27,8 +29,10 @@ target_include_directories(control_lib
   include)
 
 # Add ROS2 dependencies required by package
-ament_target_dependencies(control_lib
-  rclcpp
+target_link_libraries(control_lib
+  rclcpp::rclcpp
+  ${nav_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
 )
 
 # Create ROS2 node executable from source files

src/robot/control/include/control_node.hpp

@@ -2,6 +2,10 @@
 #define CONTROL_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
 
 #include "control_core.hpp"
 
@@ -11,6 +15,23 @@ class ControlNode : public rclcpp::Node {
 
   private:
     robot::ControlCore control_;
+    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    nav_msgs::msg::Path path;
+    nav_msgs::msg::Odometry odom;
+    geometry_msgs::msg::PoseStamped goal;
+    geometry_msgs::msg::PoseStamped target;
+
+    int64_t dt = 50; //ms
+    const double max_linear_speed = 2.0;
+    const double max_angular_speed = 2.0;
+    const double lookahead_distance = 1.5;
+
+    void periodic();
+    bool is_near(double target_x, double target_y, double tolerance);
 };
 
 #endif

src/robot/control/package.xml

@@ -10,6 +10,8 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/robot/control/src/control_node.cpp

@@ -1,6 +1,82 @@
 #include "control_node.hpp"
 
-ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {}
+ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {
+  path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+    "/path", 10, 
+    [this](const nav_msgs::msg::Path::SharedPtr msg) { 
+      path = *msg; 
+      goal = path.poses.back();
+  });
+  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "/odom/filtered", 10, 
+    [this](const nav_msgs::msg::Odometry::SharedPtr msg) { odom = *msg; });
+  cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(dt), [this]() { periodic(); });
+}
+
+void ControlNode::periodic() {
+  if (path.poses.empty()) {
+    return;
+  }
+
+  target = path.poses.front();
+
+  const size_t n = path.poses.size();
+  double distance = 0.0;
+  for (size_t i = 0; i + 1 < n; i++) {
+    double x1 = path.poses[i].pose.position.x;
+    double y1 = path.poses[i].pose.position.y;
+    double x2 = path.poses[i + 1].pose.position.x;
+    double y2 = path.poses[i + 1].pose.position.y;
+    distance += std::hypot(x1 - x2, y1 - y2);
+
+    if (distance >= lookahead_distance) {
+      target = path.poses[i + 1];
+      break;
+    }
+  }
+
+  double dx = target.pose.position.x - odom.pose.pose.position.x;
+  double dy = target.pose.position.y - odom.pose.pose.position.y;
+  double target_distance = std::hypot(dx, dy);
+  double target_heading = std::atan2(dy, dx); 
+
+  double qx = odom.pose.pose.orientation.x;
+  double qy = odom.pose.pose.orientation.y;
+  double qz = odom.pose.pose.orientation.z;
+  double qw = odom.pose.pose.orientation.w;
+  double heading = std::atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz));
+  double d_heading = target_heading - heading;
+
+  // ensure change in heading is between -pi and pi
+  d_heading = std::atan2(std::sin(d_heading), std::cos(d_heading));
+
+  // TODO: tune speed
+  double angular_speed = std::clamp(d_heading * 2.0, -max_angular_speed, max_angular_speed);
+  double linear_speed = std::clamp(target_distance * 1.2, 0.0, max_linear_speed); 
+
+  if (is_near(goal.pose.position.x, goal.pose.position.y, 0.5)) {
+    linear_speed = 0;
+    angular_speed = 0;
+  } 
+
+  geometry_msgs::msg::Twist twist_msg;
+  twist_msg.linear.x = linear_speed; 
+  twist_msg.linear.y = 0; 
+  twist_msg.linear.z = 0;
+  twist_msg.angular.x = 0;
+  twist_msg.angular.y = 0;
+  twist_msg.angular.z = angular_speed;
+
+  cmd_vel_pub_->publish(twist_msg);
+}
+
+bool ControlNode::is_near(double target_x, double target_y, double tolerance) {
+  double dx = target_x - odom.pose.pose.position.x;
+  double dy = target_y - odom.pose.pose.position.y;
+  return std::hypot(dx, dy) <= tolerance;
+}
+
 
 int main(int argc, char ** argv)
 {

src/robot/costmap/CMakeLists.txt

@@ -13,6 +13,9 @@ endif()
 # Search for dependencies required for building this package
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -25,7 +28,12 @@ add_library(costmap_lib
 target_include_directories(costmap_lib
   PUBLIC include)
 # Add ROS2 dependencies required by package
-ament_target_dependencies(costmap_lib rclcpp)
+target_link_libraries(costmap_lib PUBLIC
+  ${std_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  ${sensor_msgs_TARGETS}
+  rclcpp::rclcpp
+)
 
 # Create ROS2 node executable from source files
 add_executable(costmap_node src/costmap_node.cpp)

src/robot/costmap/include/costmap_core.hpp

@@ -1,19 +1,20 @@
 #ifndef COSTMAP_CORE_HPP_
 #define COSTMAP_CORE_HPP_
 
+#include <vector>
+#include <queue>
+
 #include "rclcpp/rclcpp.hpp"
 
 namespace robot
 {
-
 class CostmapCore {
   public:
-    // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal
-    explicit CostmapCore(const rclcpp::Logger& logger);
+  // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal
+  explicit CostmapCore(const rclcpp::Logger& logger);
 
   private:
     rclcpp::Logger logger_;
-
 };
 
 }  

src/robot/costmap/include/costmap_node.hpp

@@ -2,15 +2,42 @@
 #define COSTMAP_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp" 
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+#include <vector>
 
 #include "costmap_core.hpp"
 
 class CostmapNode : public rclcpp::Node {
+  struct Point {
+    int x, y;
+  };
+
   public:
     CostmapNode();
 
   private:
     robot::CostmapCore costmap_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_pub_;
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sensor_msg_sub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // costmap settings
+    // TODO: find most optimal constants for mapping
+    const int rows = 200;
+    const int cols = 200; 
+    const float resolution = 0.1;
+    const float width_m = cols * resolution;
+    const float height_m = rows * resolution;
+    const float inflation_radius_m = 2.0;
+    const float max_cost = 100;
+
+    std::vector<float> create_costmap(float angle_min, float angle_increment,
+      float range_min, float range_max, const std::vector<float> &ranges);
+    void sensor_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
 };
 
 #endif 

src/robot/costmap/package.xml

@@ -10,6 +10,9 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>