Merge pull request #19 from Kei18/PR-18

Pr 18

.gitignore

@@ -42,3 +42,5 @@
 
 # Generated by running openFrameworks on Mac
 openFrameworks-Info.plist
+
+bin/

README.md

@@ -9,7 +9,7 @@
 
 Simple & scalable multi-agent pathfinding (MAPF) visualizer for research usage.
 
-Based on [openFrameworks](https://openframeworks.cc/), written in C++
+Based on [openFrameworks](https://openframeworks.cc/), written in C++14.
 
 </div>
 
@@ -35,12 +35,12 @@ echo -e "\nexport PATH=$(pwd)/bin/mapf-visualizer.app/Contents/MacOS:\$PATH" >>
 
 required: around 10 minutes
 
-### for Ubuntu 20.04
+### for Ubuntu 24.04
 
 ```sh
 git clone https://github.com/Kei18/mapf-visualizer.git
 cd mapf-visualizer
-wget -O third_party/openFrameworks.tar.gz https://github.com/openframeworks/openFrameworks/releases/download/0.11.2/of_v0.11.2_linux64gcc6_release.tar.gz
+wget -O third_party/openFrameworks.tar.gz https://github.com/openframeworks/openFrameworks/releases/download/0.12.0/of_v0.12.0_linux64gcc6_release.tar.gz
 tar -xzvf third_party/openFrameworks.tar.gz -C third_party --strip-components=1 --one-top-level=openFrameworks
 sudo third_party/openFrameworks/scripts/linux/ubuntu/install_dependencies.sh
 sudo third_party/openFrameworks/scripts/linux/ubuntu/install_codecs.sh
@@ -75,6 +75,18 @@ mapf-visualizer assets/random-32-32-20.map assets/demo_random-32-32-20.txt
 
 You can manipulate it via your keyboard. See printed info.
 
+### Orientation
+
+The agent orientation is supported, thanks to [@JustinShetty](https://github.com/JustinShetty).
+Try:
+
+```sh
+mapf-visualizer assets/2x2.map assets/demo_2x2.txt
+```
+
+![](./assets/orientation.gif)
+
+
 ## Input format of planning result
 
 e.g.,
@@ -111,6 +123,6 @@ pre-commit install
 
 This software is released under the MIT License, see [LICENSE.txt](LICENCE.txt).
 
-## Author
+## Maintainer
 
 [Keisuke Okumura](https://kei18.github.io)

assets/2x2.map

@@ -0,0 +1,6 @@
+type octile
+height 2
+width 2
+map
+..
+..

assets/demo_2x2.txt

@@ -0,0 +1,6 @@
+0:(0,0,Y_MINUS),
+1:(0,0,X_PLUS),
+2:(1,0,X_PLUS),
+3:(1,0,Y_PLUS),
+4:(1,1,Y_PLUS),
+5:(1,1,X_PLUS),

include/graph.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include <string>
 #include <vector>
 
 struct Vertex {
@@ -10,8 +11,66 @@ struct Vertex {
 
   Vertex(int _id, int _index, int _x, int _y);
 };
+
+class Orientation
+{
+public:
+  enum Value {
+    NONE = 0,
+    X_MINUS = 1,
+    X_PLUS = 2,
+    Y_MINUS = 3,
+    Y_PLUS = 4,
+    NUM_ORIENTATIONS = 5
+  };
+
+  constexpr Orientation(Value v) : value(v) {}
+  constexpr bool operator==(const Orientation::Value& v) const
+  {
+    return value == v;
+  }
+  constexpr bool operator==(const Orientation& o) const
+  {
+    return value == o.value;
+  }
+  constexpr bool operator!=(const Orientation::Value& v) const
+  {
+    return value != v;
+  }
+  constexpr bool operator!=(const Orientation& o) const
+  {
+    return value != o.value;
+  }
+
+  static Orientation from_string(const std::string& s)
+  {
+    if (s == "X_MINUS") return Orientation::X_MINUS;
+    if (s == "X_PLUS") return Orientation::X_PLUS;
+    if (s == "Y_MINUS") return Orientation::Y_MINUS;
+    if (s == "Y_PLUS") return Orientation::Y_PLUS;
+    return Orientation::NONE;
+  }
+  std::string to_str() const;
+  float to_angle() const;
+
+  constexpr operator Value() const { return value; }
+  explicit operator bool() const = delete;
+
+private:
+  Value value;
+};
+
+struct Pose {
+  Vertex* v;
+  Orientation o;
+  Pose(Vertex* _v, Orientation _o) : v(_v), o(_o) {}
+  Pose(Vertex* _v) : v(_v), o(Orientation::NONE) {}
+  bool operator==(const Pose& p) const { return v == p.v && o == p.o; }
+};
+
 using Vertices = std::vector<Vertex*>;
-using Config = std::vector<Vertex*>;  // a set of locations for all agents
+using Config =
+    std::vector<Pose>;  // a set of locations and orientations for all agents
 using Solution = std::vector<Config>;
 
 struct Graph {

src/graph.cpp

@@ -1,5 +1,6 @@
 #include "../include/graph.hpp"
 
+#include <cmath>
 #include <fstream>
 #include <iostream>
 #include <regex>
@@ -92,3 +93,35 @@ Graph::Graph(char* filename) : V(Vertices()), width(0), height(0)
     }
   }
 }
+
+std::string Orientation::to_str() const
+{
+  switch (value) {
+    case X_MINUS:
+      return "X_MINUS";
+    case X_PLUS:
+      return "X_PLUS";
+    case Y_MINUS:
+      return "Y_MINUS";
+    case Y_PLUS:
+      return "Y_PLUS";
+    default:
+      return "NONE";
+  }
+}
+
+float Orientation::to_angle() const
+{
+  switch (value) {
+    case X_MINUS:
+      return 180;
+    case X_PLUS:
+      return 0;
+    case Y_MINUS:
+      return 270;
+    case Y_PLUS:
+      return 90;
+    default:
+      return NAN;
+  }
+}

src/main.cpp

@@ -5,7 +5,8 @@
 #include "../include/ofApp.hpp"
 #include "ofMain.h"
 
-const std::regex r_pos = std::regex(R"(\((\d+),(\d+)\),)");
+const std::regex r_pos =
+    std::regex(R"(\((\d+),(\d+),?([XY]{1}_[A-Z]{4,5})?\),)");
 
 int main(int argc, char *argv[])
 {
@@ -28,7 +29,7 @@ int main(int argc, char *argv[])
   std::smatch m, results;
   while (getline(solution_file, line)) {
     // so we only process real solution lines
-    if(line.find(":(") == std::string::npos) continue;
+    if (line.find(":(") == std::string::npos) continue;
 
     auto iter = line.cbegin();
     Config c;
@@ -37,7 +38,11 @@ int main(int argc, char *argv[])
       if (std::regex_search(iter, search_end, m, r_pos)) {
         auto x = std::stoi(m[1].str());
         auto y = std::stoi(m[2].str());
-        c.push_back(G.U[G.width * y + x]);
+        Orientation o = Orientation::NONE;
+        if (m[3].matched) {
+          o = Orientation::from_string(m[3].str());
+        }
+        c.push_back(Pose(G.U[G.width * y + x], o));
         iter += m[0].length();
       } else {
         break;

src/ofApp.cpp

@@ -130,7 +130,8 @@ void ofApp::draw()
       auto y_draw =
           y * scale - scale / 2 + window_y_top_buffer + scale / 2 - 0.15;
       auto gridline_space = flg_grid ? 0.3 : 0.0;
-      ofDrawRectangle(x_draw, y_draw, scale - gridline_space, scale - gridline_space);
+      ofDrawRectangle(x_draw, y_draw, scale - gridline_space,
+                      scale - gridline_space);
       if (flg_font) {
         ofSetColor(Color::font);
         font.drawString(std::to_string(index), x_draw + 1,
@@ -143,10 +144,20 @@ void ofApp::draw()
   if (flg_goal) {
     for (int i = 0; i < N; ++i) {
       ofSetColor(Color::agents[i % Color::agents.size()]);
-      auto g = goals[i];
+      auto g = goals[i].v;
+      auto o = goals[i].o;
       int x = g->x * scale + window_x_buffer + scale / 2;
       int y = g->y * scale + window_y_top_buffer + scale / 2;
       ofDrawRectangle(x - goal_rad / 2, y - goal_rad / 2, goal_rad, goal_rad);
+
+      if (o != Orientation::NONE) {
+        ofSetColor(255, 255, 255);
+        ofPushMatrix();
+        ofTranslate(x, y);
+        ofRotateZDeg(o.to_angle());
+        ofDrawTriangle(0, goal_rad / 2, 0, -goal_rad / 2, goal_rad / 2, 0);
+        ofPopMatrix();
+      }
     }
   }
 
@@ -157,14 +168,26 @@ void ofApp::draw()
     auto t2 = t1 + 1;
 
     // agent position
-    auto v = P->at(t1)[i];
+    auto p_t1 = P->at(t1)[i];
+    auto v = p_t1.v;
+    auto o = p_t1.o;
     float x = v->x;
     float y = v->y;
+    float angle = o.to_angle();
 
     if (t2 <= T) {
-      auto u = P->at(t2)[i];
+      auto p_t2 = P->at(t2)[i];
+      auto u = p_t2.v;
       x += (u->x - x) * (timestep_slider - t1);
       y += (u->y - y) * (timestep_slider - t1);
+
+      if (o != Orientation::NONE) {
+        float angle_next = p_t2.o.to_angle();
+        float diff = angle_next - angle;
+        if (diff > 180.0f) diff -= 360.0f;
+        if (diff < -180.0f) diff += 360.0f;
+        angle += diff * (timestep_slider - t1);
+      }
     }
     x *= scale;
     y *= scale;
@@ -175,19 +198,19 @@ void ofApp::draw()
 
     // goal
     if (line_mode == LINE_MODE::STRAIGHT) {
-      ofDrawLine(goals[i]->x * scale + window_x_buffer + scale / 2,
-                 goals[i]->y * scale + window_y_top_buffer + scale / 2, x, y);
+      ofDrawLine(goals[i].v->x * scale + window_x_buffer + scale / 2,
+                 goals[i].v->y * scale + window_y_top_buffer + scale / 2, x, y);
     } else if (line_mode == LINE_MODE::PATH) {
       // next loc
       ofSetLineWidth(2);
       if (t2 <= T) {
-        auto u = P->at(t2)[i];
+        auto u = P->at(t2)[i].v;
         ofDrawLine(x, y, u->x * scale + window_x_buffer + scale / 2,
                    u->y * scale + window_y_top_buffer + scale / 2);
       }
       for (int t = t1 + 1; t < T; ++t) {
-        auto v_from = P->at(t)[i];
-        auto v_to = P->at(t + 1)[i];
+        auto v_from = P->at(t)[i].v;
+        auto v_to = P->at(t + 1)[i].v;
         if (v_from == v_to) continue;
         ofDrawLine(v_from->x * scale + window_x_buffer + scale / 2,
                    v_from->y * scale + window_y_top_buffer + scale / 2,
@@ -198,11 +221,21 @@ void ofApp::draw()
     }
 
     // agent at goal
-    if (v == goals[i]) {
+    if (p_t1 == goals[i]) {
       ofSetColor(255, 255, 255);
       ofDrawCircle(x, y, agent_rad * 0.7);
     }
 
+    // agent orientation
+    if (o != Orientation::NONE) {
+      ofSetColor(255, 255, 255);
+      ofPushMatrix();
+      ofTranslate(x, y);
+      ofRotateZDeg(angle);
+      ofDrawTriangle(0, agent_rad, 0, -agent_rad, agent_rad, 0);
+      ofPopMatrix();
+    }
+
     // id
     if (flg_font) {
       ofSetColor(Color::font);