Implemented odometry logic and pathplanning.

READMEtest.md

@@ -0,0 +1,29 @@
+# r26_test Submission
+
+## Solution
+The task involved two main problems: 
+1. **Path Planning** – Compute a valid path from a start to a goal position on a 2D grid while avoiding obstacles.
+2. **Odometry** – Compute motion commands (time to traverse and total rotation) for a robot along the planned path using wheel radius and RPM.
+
+## Thought Process
+- First, I analyzed the problem and split it into **path planning** and **odometry**.  
+- For path planning, I decided on a **4-connected A* algorithm** using Euclidean distance as a heuristic.  
+- For odometry, I computed **linear velocity** from wheel radius and RPM, then calculated **distance and heading changes** between consecutive points.  
+- I ensured angle changes were wrapped between -180° and 180° to correctly sum rotations.
+
+## Implementation
+- Created `Planner` class for path planning and `Odometry` class for motion commands.  
+- Used vectors and pairs to store paths and positions.  
+- Tested the implementation on example grids and start/goal positions, verified outputs manually.  
+- Handled minor compile issues such as `M_PI` by defining pi as 3.141592653589793.  
+
+## Challenges
+- Angle wrapping in odometry required careful handling to avoid incorrect rotation sums.  
+- Path reconstruction in A* needed careful indexing to reverse the path correctly.  
+- Initially faced compile issues on Windows due to `M_PI` not being defined.
+
+## Resources Used
+- C++ documentation for `atan2`, `sqrt`, `vector`.  
+- GitHub Docs for creating Pull Requests.  
+- AI assistance to understand odometry formulas and path planning structure.  
+- Online references for A* algorithm and grid representation.

src/gridmap.cpp

@@ -1,6 +1,8 @@
 #include "gridmap.h"
 #include <iostream>
 
+
+
 using namespace std;
 
 Gridmapper::Gridmapper(GPS origin, double cellsize, int rows, int cols)
@@ -23,7 +25,8 @@ pair<int, int> Gridmapper::gpstogrid(const GPS &point) const {
 
 const vector<vector<bool>> &Gridmapper::getGrid() const { return grid; }
 
-double Gridmapper::deg2rad(double deg) { return deg * M_PI / 180.0; }
+double Gridmapper::deg2rad(double deg) { return deg *3.14159265358979323846
+  / 180.0; }
 
 bool Gridmapper::isvalid(int row, int col) const {
   return (row >= 0 && row < rows && col >= 0 && col < cols);

src/main.cpp

@@ -7,11 +7,13 @@
 #include <iostream>
 #include <string>
 
+
 using namespace std;
 
 // Helper to convert angle to unit direction
 pair<double, double> directionFromAngle(double angle_deg) {
-  double rad = angle_deg * M_PI / 180.0;
+  double rad = angle_deg * 3.14159265358979323846
+ / 180.0;
   return {cos(rad), sin(rad)};
 }
 

src/odometry.cpp

@@ -10,7 +10,8 @@ Odometry::Odometry(double wheel_radius, double rpm)
     : radius(wheel_radius), rpm(rpm) {
   // Linear velocity (m/s) =(wheel circumference * revolutions per second)
   double rps = rpm / 60.0;
-  linear_vel = 2 * M_PI * radius * rps;
+  linear_vel = 2 *3.14159265358979323846
+* radius * rps;
 }
 
 double Odometry::distance(int x1, int y1, int x2, int y2) {
@@ -19,14 +20,38 @@ double Odometry::distance(int x1, int y1, int x2, int y2) {
 
 double Odometry::angle(int x1, int y1, int x2, int y2) {
   // atan2 returns radians, convert to degrees
-  return atan2(y2 - y1, x2 - x1) * 180.0 / M_PI;
+  return atan2(y2 - y1, x2 - x1) * 180.0 / 3.14159265358979323846
+;
 }
 
 MotionCommand Odometry::computeCommands(vector<pair<int, int>> &path) {
 
   MotionCommand res = {0.0, 0.0}; // store total time and angle traversed
 
- /* Implement you odometry logic here */ 
+ /* Implement you odometry logic here */
+  if (path.size() < 2) return res;
+
+  double prev_heading = angle(path[0].first, path[0].second,
+                              path[1].first, path[1].second);
+
+  for (size_t i = 1; i < path.size(); i++) {
+    // distance between consecutive points
+    double dist = distance(path[i-1].first, path[i-1].second,
+                           path[i].first, path[i].second);
+    res.time_sec += dist / linear_vel;
+
+    // current heading
+    double heading = angle(path[i-1].first, path[i-1].second,
+                           path[i].first, path[i].second);
+
+    // change in heading
+    double dtheta = heading - prev_heading;
+    while (dtheta > 180.0) dtheta -= 360.0;
+    while (dtheta < -180.0) dtheta += 360.0;
+
+    res.angle_deg += fabs(dtheta);
+    prev_heading = heading;
+  }
 
   return res;
 }

src/planning.cpp

@@ -22,6 +22,67 @@ vector<pair<int, int>> Planner::pathplanning(pair<int, int> start,
   vector<pair<int, int>> path; // store final path 
 
   /* Implement Path Planning logic here */
+  if (!isvalid(start.first, start.second) || !isvalid(goal.first, goal.second))
+    return path;
+
+  // movement directions (4-connected grid)
+  int dx[4] = {-1, 1, 0, 0};
+  int dy[4] = {0, 0, -1, 1};
+
+  vector<vector<double>> gscore(rows, vector<double>(cols, 1e9));
+  vector<vector<pair<int, int>>> parent(rows, vector<pair<int, int>>(cols, {-1, -1}));
+
+  struct Node { int x, y; double f, g; };
+  vector<Node> open;
+
+  int sx = start.first, sy = start.second;
+  int gx = goal.first, gy = goal.second;
+
+  gscore[sx][sy] = 0.0;
+  open.push_back({sx, sy, heuristic(sx, sy, gx, gy), 0.0});
+
+  while (!open.empty()) {
+    // find node with smallest f
+    int best_idx = 0;
+    for (int i = 1; i < (int)open.size(); i++) {
+      if (open[i].f < open[best_idx].f)
+        best_idx = i;
+    }
+    Node cur = open[best_idx];
+    open.erase(open.begin() + best_idx);
+
+    if (cur.x == gx && cur.y == gy) {
+      // reconstruct path backwards
+      int cx = gx, cy = gy;
+      vector<pair<int, int>> revpath;
+      while (!(cx == sx && cy == sy)) {
+        revpath.push_back({cx, cy});
+        auto p = parent[cx][cy];
+        cx = p.first;
+        cy = p.second;
+      }
+      revpath.push_back({sx, sy});
+      // reverse manually
+      for (int i = (int)revpath.size() - 1; i >= 0; i--)
+        path.push_back(revpath[i]);
+      return path;
+    }
+
+    for (int k = 0; k < 4; k++) {
+      int nx = cur.x + dx[k];
+      int ny = cur.y + dy[k];
+
+      if (!isvalid(nx, ny)) continue;
+
+      double tentative_g = gscore[cur.x][cur.y] + 1.0;
+      if (tentative_g < gscore[nx][ny]) {
+        gscore[nx][ny] = tentative_g;
+        parent[nx][ny] = {cur.x, cur.y};
+        double f = tentative_g + heuristic(nx, ny, gx, gy);
+        open.push_back({nx, ny, f, tentative_g});
+      }
+    }
+  }
 
   return path;
 }