first draft of assignment done

Author: Edwardius

pages/admission_assignments/asd_admission_assignment.mdx

@@ -269,7 +269,7 @@ So how do you use ROS? Learning ROS (and robotics programming in general) takes
 ### ROS System Diagram
 Here's a simple diagram of three ROSnodes talking to each other. This is the generic way most robotics developers look at robotics systems.
 
-![]()
+![](../../public/assets/simple_ros_diagram.png)
 
 ### ROS Node
 A process that is running instances of ROS objects is called a ROSnode. What this means is, as long as your code has ROS objects that listen/publish data to the network, then it is considered a ROSnode. You can think of ROSnodes as the building blocks of a robotics system.
@@ -297,7 +297,7 @@ In the warmup, we made you write a simple publisher. Please use the warmup code
 ### The Sky's the Limit
 A robotics system can be composed of a bunch of different ROSnodes communcating in a bunch of different ways. In fact, a ROSnode can consist of multiple publishers and subscribers. You just need to be careful of the concurrency between them.
 
-![]()
+![](../../public/assets/limitless_potential.png)
 
 Unfortunately, the only way to truly understand ROS is to read their docs and use it. If you have any questions when programming ROS, please ask them in our Discord.
 
@@ -374,7 +374,7 @@ From before, you know that this robot will need the following high-level modules
 ### A Simple Navigation Architecture
 Below is a diagram of a simple navigation architecture that can be used to help the robot navigate from point A to point B.
 
-![]()
+![](../../public/assets/assignment_arch.png)
 
 Here is a description of each node:
 - **Costmap:** A ROSnode that takes in LaserScans from the `/lidar` topic, and converts them into a costmap. This is a discretized grid of squares that represent the chances as object exists in that grid with an arbitrary score.
@@ -859,6 +859,7 @@ private:
 > - 1 Subscriber that subscribes to the '/path' topic for [nav_msgs::msg::Path](https://docs.ros.org/en/lunar/api/nav_msgs/html/msg/Path.html) messages
 > - 1 Subscriber that subscribes to the '/odom/filtered' topic for [nav_msgs::msg::Odometry](https://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages
 > - 1 Publisher that publishes [geometry_msgs::msg::Twist](https://docs.ros.org/en/lunar/api/geometry_msgs/html/msg/Twist.html) messages to a '/costmap' topic
+> - 1 Timer to follow a stored path
 
 ![]()
 
@@ -880,130 +881,152 @@ The lookahead distance controls how far ahead the target point is chosen. A larg
 
 You can read more about Pure Pursuit [here](https://www.mathworks.com/help/nav/ug/pure-pursuit-controller.html).
 
----
-
-#### **Node Behavior**
-
-The control node continuously tracks the robot's position relative to the path and calculates how the robot should move to reach the next "lookahead point." The Pure Pursuit algorithm determines the steering curvature needed to bring the robot closer to this point, ensuring smooth and dynamic path tracking.
-
-#### **Core Functionalities**
+#### **Node Behavior and Design**
 
-1. **Path Tracking**:
-   - Subscribes to `/path` (`nav_msgs::msg::Path`) for the global path generated by the planner.
-   - Selects a "lookahead point" on the path that lies a fixed distance ahead of the robot.
+The Pure Pursuit Controller node is responsible for taking a path and odometry data as inputs and generating velocity commands to guide the robot along the path. The node operates in a closed-loop fashion: it continuously checks the robot's current position, calculates the desired heading based on the "lookahead point," and generates appropriate linear and angular velocity commands.
 
-2. **Robot Pose Tracking**:
-   - Subscribes to `/odom/filtered` (`nav_msgs::msg::Odometry`) to get the robot's current position and orientation.
+The node subscribes to:
+- **`/path`**: Receives the global path as a series of waypoints.
+- **`/odom/filtered`**: Tracks the robot's current position and orientation.
 
-3. **Twist Calculation**:
-   - Computes linear and angular velocities:
-     - Linear velocity is proportional to the distance to the lookahead point.
-     - Angular velocity is determined based on the curvature required to reach the lookahead point.
-   - Publishes the velocity commands as a `geometry_msgs::msg::Twist` message to the `/cmd_vel` topic.
+The node publishes:
+- **`/cmd_vel`**: Outputs velocity commands in the form of linear and angular velocities.
 
-4. **Dynamic Behavior**:
-   - Continuously updates the lookahead point and recalculates velocities as the robot moves.
+Additionally, the node employs a timer to ensure regular updates of the velocity commands.
 
 ---
 
-#### **Key Pure Pursuit Components**
+#### **Key Functionalities**
+
+1. **Path Tracking**: 
+   - Extract the current lookahead point from the path based on the robot’s position and a fixed lookahead distance.
+   - Compute the steering angle required to reach the lookahead point.
 
-- **Lookahead Point**: A target point chosen on the path at a fixed distance ahead of the robot.
-- **Steering Angle**: Computed based on the curvature of the circular arc connecting the robot to the lookahead point.
-- **Curvature**: Determines the angular velocity needed to follow the circular arc.
+2. **Velocity Commands**:
+   - Calculate linear and angular velocities using the steering angle and the robot’s dynamics.
 
-The steering curvature is calculated as:
-```math
-\kappa = \frac{2 \cdot y}{L_d^2}
-```
-- y: Lateral error (perpendicular distance to the lookahead point).
-- L_d: Lookahead distance.
+3. **Error Handling**:
+   - Handle edge cases like when the path is empty or the robot is close to the final goal.
 
-The angular velocity is then:
-```math
-\omega = v \cdot \kappa
-```
-- v: Linear velocity (proportional to L_d).
+4. **Timer for Updates**:
+   - The node updates the velocity commands at a fixed rate (e.g., 10 Hz) for smooth and consistent control.
 
 ---
 
-#### **Pseudo-ROS Node Implementation**
+#### **Pseudo ROS Code Example**
 
 ```cpp
-class ControlNode : public rclcpp::Node {
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <cmath>
+#include <optional>
+
+class PurePursuitController : public rclcpp::Node {
 public:
-    ControlNode() : Node("control_node"), lookahead_distance_(1.0), max_linear_velocity_(0.5) {
-        // Subscribers
+    PurePursuitController() : Node("pure_pursuit_controller") {
+        // Initialize parameters
+        lookahead_distance_ = 1.0;  // Lookahead distance
+        goal_tolerance_ = 0.1;     // Distance to consider the goal reached
+        linear_speed_ = 0.5;       // Constant forward speed
+
+        // Subscribers and Publishers
         path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
-            "/path", 10, std::bind(&ControlNode::pathCallback, this, std::placeholders::_1));
+            "/path", 10, [this](const nav_msgs::msg::Path::SharedPtr msg) { current_path_ = msg; });
+
         odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
-            "/odom/filtered", 10, std::bind(&ControlNode::odomCallback, this, std::placeholders::_1));
+            "/odom/filtered", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { robot_odom_ = msg; });
 
-        // Publisher
         cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
 
-        RCLCPP_INFO(this->get_logger(), "Control node initialized");
+        // Timer
+        control_timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(100), [this]() { controlLoop(); });
     }
 
 private:
-    // Subscribers and Publisher
-    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_;
+    void controlLoop() {
+        // Skip control if no path or odometry data is available
+        if (!current_path_ || !robot_odom_) {
+            return;
+        }
 
-    // Data storage
-    nav_msgs::msg::Path current_path_;
-    geometry_msgs::msg::Pose current_pose_;
+        // Find the lookahead point
+        auto lookahead_point = findLookaheadPoint();
+        if (!lookahead_point) {
+            return;  // No valid lookahead point found
+        }
 
-    // Parameters
-    double lookahead_distance_;
-    double max_linear_velocity_;
+        // Compute velocity command
+        auto cmd_vel = computeVelocity(*lookahead_point);
 
-    void pathCallback(const nav_msgs::msg::Path::SharedPtr msg) {
-        current_path_ = *msg;
-        RCLCPP_INFO(this->get_logger(), "Path received with %zu poses", current_path_.poses.size());
+        // Publish the velocity command
+        cmd_vel_pub_->publish(cmd_vel);
     }
 
-    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
-        current_pose_ = msg->pose.pose;
-        computeAndPublishTwist();
+    std::optional<geometry_msgs::msg::PoseStamped> findLookaheadPoint() {
+        // TODO: Implement logic to find the lookahead point on the path
+        return std::nullopt;  // Replace with a valid point when implemented
     }
 
-    void computeAndPublishTwist() {
-        if (current_path_.poses.empty()) {
-            RCLCPP_WARN(this->get_logger(), "No path available");
-            return;
-        }
-
-        geometry_msgs::msg::PoseStamped lookahead_point = findLookaheadPoint();
-        if (!lookahead_point.header.frame_id.empty()) {
-            geometry_msgs::msg::Twist cmd_vel;
-            calculatePurePursuit(lookahead_point, cmd_vel);
-            cmd_vel_pub_->publish(cmd_vel);
-        } else {
-            RCLCPP_WARN(this->get_logger(), "No valid lookahead point found");
-        }
+    geometry_msgs::msg::Twist computeVelocity(const geometry_msgs::msg::PoseStamped &target) {
+        // TODO: Implement logic to compute velocity commands
+        geometry_msgs::msg::Twist cmd_vel;
+        return cmd_vel;
     }
 
-    geometry_msgs::msg::PoseStamped findLookaheadPoint() {
-        // TODO: Implement lookahead point selection logic
-        return geometry_msgs::msg::PoseStamped(); // Placeholder
+    double computeDistance(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b) {
+        // TODO: Implement distance calculation between two points
+        return 0.0;
     }
 
-    void calculatePurePursuit(const geometry_msgs::msg::PoseStamped &lookahead_point,
-                              geometry_msgs::msg::Twist &cmd_vel) {
-        // TODO: Implement Pure Pursuit control logic
+    double extractYaw(const geometry_msgs::msg::Quaternion &quat) {
+        // TODO: Implement quaternion to yaw conversion
+        return 0.0;
     }
+
+    // Subscribers and Publishers
+    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_;
+
+    // Timer
+    rclcpp::TimerBase::SharedPtr control_timer_;
+
+    // Data
+    nav_msgs::msg::Path::SharedPtr current_path_;
+    nav_msgs::msg::Odometry::SharedPtr robot_odom_;
+
+    // Parameters
+    double lookahead_distance_;
+    double goal_tolerance_;
+    double linear_speed_;
 };
+
+// Main function
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<PurePursuitController>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
 ```
 
 ---
 
 #### **Key Details**
-1. **Dynamic Path Tracking**: Continuously adjusts to changes in the path and robot's position.
-2. **Smooth Control**: The Pure Pursuit algorithm ensures smooth motion by steering toward the lookahead point.
-3. **Lookahead Distance**: A tunable parameter that balances tracking accuracy and smoothness.
-4. **Twist Output**: Publishes both linear and angular velocities, which can be consumed by the robot's hardware abstraction layer (in our case, our simulation).
+
+- **Lookahead Distance**: The controller selects a point on the path at a fixed distance ahead of the robot. This distance must be tuned for the robot's dynamics and desired path-following performance.
+  
+- **Linear Speed**: The robot’s speed is typically kept constant, while the angular velocity adjusts based on the curvature.
+
+- **Edge Cases**: Ensure the node handles scenarios like an empty path or reaching the final goal. You can stop the robot when the goal is within the `goal_tolerance`.
+
+- **Yaw Calculation**: Converting the robot’s quaternion orientation to yaw is essential for computing the steering angle.
+
+This implementation provides a simple yet effective way to integrate Pure Pursuit Control in ROS for smooth path following.
 
 ## Helpful Tips
 - You never need to run `colcon build` because our monorepo infrastructure runs it automatically whenever you run `./watod build`. A good amount of ROS packaging and Launchfiles has also been abstracted away from you. This was deliberate in order to make your experience with the assignment as "code-heavy" as possible.