commit aa5e4ddcdd38a8388d7157691894c250e3cb439d
Author: Leon <leon@leon-VirtualBox.(none)>
Date:   Tue Feb 14 18:15:26 2012 -0500

    Finishing BFS for behaviors navigationMap. Also commiting teleop skeleton code.

diff --git a/scout/libscout/src/behaviors/navigationMap.cpp b/scout/libscout/src/behaviors/navigationMap.cpp
index 6da9505..e2590bb 100644
--- a/scout/libscout/src/behaviors/navigationMap.cpp
+++ b/scout/libscout/src/behaviors/navigationMap.cpp
@@ -14,53 +14,53 @@ navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname)
    *     ---         ---        ---       ---
    */
   
-    Edge* a1 = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
-                MAKE_EDGE(IRIGHT, 9, 40),
-                MAKE_EDGE(IUTURN, DEADEND, 0)};
+    Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
+                            MAKE_EDGE(IRIGHT, 9, 40),
+                            MAKE_EDGE(IUTURN, DEADEND, 0)};
 
-    Edge* a2 = {MAKE_EDGE(ISTRAIGHT, 3, 10),
-                MAKE_EDGE(IRIGHT, 10, 40),
-                MAKE_EDGE(IUTURN, 5, 10)};
+    Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10),
+                            MAKE_EDGE(IRIGHT, 10, 40),
+                            MAKE_EDGE(IUTURN, 5, 10)};
 
-    Edge* a3 = {MAKE_EDGE(ISTRAIGHT, 4, 10),
-                MAKE_EDGE(IRIGHT, 11, 40),
-                MAKE_EDGE(IUTURN, 6, 10)};
+    Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10),
+                            MAKE_EDGE(IRIGHT, 11, 40),
+                            MAKE_EDGE(IUTURN, 6, 10)};
 
-    Edge* a4 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
-                MAKE_EDGE(IRIGHT, 12, 40),
-                MAKE_EDGE(IUTURN, 7, 10)};
+    Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
+                            MAKE_EDGE(IRIGHT, 12, 40),
+                            MAKE_EDGE(IUTURN, 7, 10)};
 
-    Edge* a5 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
-                MAKE_EDGE(ILEFT, 9, 40),
-                MAKE_EDGE(IUTURN, 2, 10)};
+    Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
+                            MAKE_EDGE(ILEFT, 9, 40),
+                            MAKE_EDGE(IUTURN, 2, 10)};
 
-    Edge* a6 = {MAKE_EDGE(ISTRAIGHT, 5, 10),
-                MAKE_EGE(ILEFT, 10, 40),
-                MAKE_EDGE(IUTURN, 3, 10)};
+    Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10),
+                            MAKE_EDGE(ILEFT, 10, 40),
+                            MAKE_EDGE(IUTURN, 3, 10)};
 
-    Edge* a7 = {MAKE_EDGE(ISTRAIGHT, 6, 10),
-                MAKE_EGE(ILEFT, 11, 40),
-                MAKE_EDGE(IUTURN, 4, 10)};
+    Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10),
+                            MAKE_EDGE(ILEFT, 11, 40),
+                            MAKE_EDGE(IUTURN, 4, 10)};
 
-    Edge* a8 = {MAKE_EDGE(ISTRAIGHT, 7, 10),
-                MAKE_EGE(ILEFT, 12, 40),
-                MAKE_EDGE(IUTURN, DEADEND, 0)};
+    Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10),
+                            MAKE_EDGE(ILEFT, 12, 40),
+                            MAKE_EDGE(IUTURN, DEADEND, 0)};
 
-    Edge* a9 = {MAKE_EDGE(IRIGHT, 2, 10),
-                MAKE_EGE(ILEFT, DEADEND, 0),
-                MAKE_EDGE(IUTURN, 9, 40)};
+    Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10),
+                            MAKE_EDGE(ILEFT, DEADEND, 0),
+                            MAKE_EDGE(IUTURN, 9, 40)};
 
-    Edge* a10 = {MAKE_EDGE(IRIGHT, 3, 10),
-                MAKE_EGE(ILEFT, 5, 10),
-                MAKE_EDGE(IUTURN, 10, 40)};
+    Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10),
+                            MAKE_EDGE(ILEFT, 5, 10),
+                            MAKE_EDGE(IUTURN, 10, 40)};
 
-    Edge* a11 = {MAKE_EDGE(IRIGHT, 4, 10),
-                MAKE_EGE(ILEFT, 6, 10),
-                MAKE_EDGE(IUTURN, 11, 40)};
+    Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10),
+                            MAKE_EDGE(ILEFT, 6, 10),
+                            MAKE_EDGE(IUTURN, 11, 40)};
 
-    Edge* a12 = {MAKE_EDGE(IRIGHT, DEADEND, 0),
-                MAKE_EGE(ILEFT, 7, 10),
-                MAKE_EDGE(IUTURN, 12, 40)};
+    Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),
+                            MAKE_EDGE(ILEFT, 7, 10),
+                            MAKE_EDGE(IUTURN, 12, 40)};
       
     map.pushback(a1);
     map.pushback(a2);
@@ -82,11 +82,8 @@ navigationMap::~navigationMap()
 {
     while(!map.empty())
     {
-      Edge** e = pop_back();
-      for(int i=0; i<ARRAY_SIZE; i++)
-      {
-        delete e[i];
-      }
+      Edge* e = map.pop_back();
+      delete e;
     }
     return;
 }
@@ -106,26 +103,32 @@ navigationMap::shortest_path(State target_state)
     // BFS algorithm
     State curr_state = get_state();
     char visited[MAX_NODES+1]; 
-    //FAKE QUEUE OH NO 
-    //TODO: real implementation
-    Queue queue;
-    queue.enqueue(curr_state);
+    queue<State> q;
+    q.push(curr_state);
+    // not zero = visited, zero = unvisited, negative = start state
+    visited[curr_state] = -1;
 
-    // 1 == visited, 0 == not visited
-    visited[curr_state] = 1;
-
-    while (!queue.is_empty)
+    while (!q.is_empty)
     {
-        State state = queue.dequeue();
+        State state = q.pop();
         if (state == target_state)
         {   
-            //TODO: implement moves_list
-            State i;
-
-            while(i != curr_state)
+            Turn moves_list[MAX_NODES];
+            int j = 0; // counter
+            for(State child = state; visited[child] >= 0; 
+                    child = visited[child]) //while not start state
             {
-                i = visited[i];
-
+                State parent = visited[child];
+                Edge* edges = get_outbound_edges(parent);
+                for (int i = 0; i < ARRAY_SIZE; i++)
+                {
+                    if (GET_EDGE_STATE(edges[i]) == child)
+                    {
+                        moves_list[j] = GET_EDGE_DIR(edges[i]);
+                        j++;
+                        break;
+                    }
+                }
             }
             return moves_list;
         }
@@ -136,18 +139,12 @@ navigationMap::shortest_path(State target_state)
             State new_state = GET_EDGE_STATE(edges[i]);
             if (!visited[new_state]) 
             {
+                // set visited to the parent of the new state
                 visited[new_state] = state;
-                queue.enqueue(new_state);
+                q.push(new_state);
             }
         }
     }
-}
-
-navigationMap::make_edge(Turn dir, State state, int dist)
-{
-    Edge* e = new Edge;
-    e->direction = dir;
-    e->state = state;
-    e->distance = dist;
-    return e;
+    //oops, no way to get to target from state
+    return NULL;
 }
diff --git a/scout/libscout/src/behaviors/navigationMap.h b/scout/libscout/src/behaviors/navigationMap.h
index 32e2f2a..9f6df97 100644
--- a/scout/libscout/src/behaviors/navigationMap.h
+++ b/scout/libscout/src/behaviors/navigationMap.h
@@ -2,7 +2,8 @@
 #define _NAVIGATION_MAP_
 
 #include <cstdlib>
-#include "linieDrive.h" // Get turn Macros
+#include <queue>
+#include "lineDrive.h" // Get turn Macros
 
 #define START_STATE 1
 
@@ -20,7 +21,7 @@
 
 #define MAKE_EDGE(dir, state, dist) \
 SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist)
-
+    
 typedef int Edge;
 typedef int State;
 typedef int Turn;
@@ -41,8 +42,5 @@ class navigationMap : Behavior
     private:
         vector <Edge*> map;
         State curr_state;
-
-        Edge* make_edge(Turn dir, State state, int dist);
-        
 };
 #endif
diff --git a/scout/scoutsim/CMakeLists.txt b/scout/scoutsim/CMakeLists.txt
index cd79b10..28ef539 100644
--- a/scout/scoutsim/CMakeLists.txt
+++ b/scout/scoutsim/CMakeLists.txt
@@ -36,3 +36,4 @@ target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})
 #rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
 #rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
 #rosbuild_add_executable(mimic tutorials/mimic.cpp)
+rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)
diff --git a/scout/scoutsim/src/scout_teleop.cpp b/scout/scoutsim/src/scout_teleop.cpp
new file mode 100644
index 0000000..6e69665
--- /dev/null
+++ b/scout/scoutsim/src/scout_teleop.cpp
@@ -0,0 +1,153 @@
+#include <ros/ros.h>
+#include <motors/set_motors.h>
+#include <signal.h>
+#include <termios.h>
+#include <stdio.h>
+
+// TODO: add this to a constants.h
+#define QUEUE_SIZE 10
+
+#define KEYCODE_R 0x43 
+#define KEYCODE_L 0x44
+#define KEYCODE_U 0x41
+#define KEYCODE_D 0x42
+#define KEYCODE_Q 0x71
+#define KEYCODE_S 0x73
+
+class TeleopScout
+{
+public:
+    TeleopScout();
+    void keyLoop();
+
+private:
+    ros::NodeHandle nh_;
+    bool fl_set_, fr_set_, bl_set_, br_set_;
+    int fl_speed_, fr_speed_, bl_speed_, br_speed_;
+    char last_key;
+    ros::Publisher motors_pub_;
+};
+
+TeleopScout::TeleopScout()
+{
+    fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
+    fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
+    motors_pub_ = nh_.advertise<motors::set_motors>(
+                    "scout1/set_motors", QUEUE_SIZE);
+}
+
+int kfd = 0;
+struct termios cooked, raw;
+
+void quit(int sig)
+{
+    tcsetattr(kfd, TCSANOW, &cooked);
+    ros::shutdown();
+    exit(0);
+}
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "teleop_scout");
+  TeleopScout teleop_scout;
+
+  signal(SIGINT,quit);
+
+  teleop_scout.keyLoop();
+  
+  return(0);
+}
+
+
+void TeleopScout::keyLoop()
+{
+  char c;
+  bool dirty=false;
+
+  // get the console in raw mode                                                              
+  tcgetattr(kfd, &cooked);
+  memcpy(&raw, &cooked, sizeof(struct termios));
+  raw.c_lflag &=~ (ICANON | ECHO);
+  // Setting a new line, then end of file                         
+  raw.c_cc[VEOL] = 1;
+  raw.c_cc[VEOF] = 2;
+  tcsetattr(kfd, TCSANOW, &raw);
+
+  puts("Reading from keyboard");
+  puts("---------------------------");
+  puts("Use arrow keys to move the scout.");
+
+
+  for(;;)
+  {
+    ROS_INFO("loop pass engaged");
+    // get the next event from the keyboard  
+    if(read(kfd, &c, 1) < 0)
+    {
+      perror("read():");
+      exit(-1);
+    }
+
+    //ROS_DEBUG("value: 0x%02X\n", c);
+    //ROS_INFO("value: 0x%02X\n", c);
+    switch(c)
+    {
+      case KEYCODE_L:
+        ROS_INFO("LEFT");
+        fl_speed_ = bl_speed_ = 200;
+        fr_speed_ = br_speed_ = -200;
+        dirty = true;
+        break;
+      case KEYCODE_R:
+        ROS_INFO("RIGHT");
+        fl_speed_ = bl_speed_ = -200;
+        fr_speed_ = br_speed_ = 200;
+        dirty = true;
+        break;
+      case KEYCODE_U:
+        ROS_INFO("UP");
+        fl_speed_ = bl_speed_ = 200;
+        fr_speed_ = br_speed_ = 200;
+        dirty = true;
+        break;
+      case KEYCODE_D:
+        ROS_INFO("DOWN");
+        fl_speed_ = bl_speed_ = -200;
+        fr_speed_ = br_speed_ = -200;
+        dirty = true;
+        break;
+      default:
+        ROS_INFO("test");
+        fl_speed_ = bl_speed_ = 0;
+        fr_speed_ = br_speed_ = 0;
+        dirty = true;
+        break;
+    }
+   
+    motors::set_motors msg;
+    msg.fl_set = fl_set_;
+    msg.fr_set = fr_set_;
+    msg.bl_set = bl_set_;
+    msg.br_set = br_set_;
+    msg.fl_speed = fl_speed_;
+    msg.fr_speed = fr_speed_;
+    msg.bl_speed = bl_speed_;
+    msg.br_speed = br_speed_;
+    
+    if(dirty == true)
+    {
+      if (last_key != c)
+      {
+        motors_pub_.publish(msg);
+      }
+      dirty=false;
+    }
+    last_key = c;
+  }
+
+  return;
+}
+
+
+
diff --git a/scout/scoutsim/src/turtle_teleop.cpp b/scout/scoutsim/src/turtle_teleop.cpp
new file mode 100644
index 0000000..295603f
--- /dev/null
+++ b/scout/scoutsim/src/turtle_teleop.cpp
@@ -0,0 +1,136 @@
+#include <ros/ros.h>
+#include <signal.h>
+#include <termios.h>
+#include <stdio.h>
+#include "constants.h"
+
+#define KEYCODE_R 0x43 
+#define KEYCODE_L 0x44
+#define KEYCODE_U 0x41
+#define KEYCODE_D 0x42
+#define KEYCODE_Q 0x71
+
+class TeleopTurtle
+{
+public:
+  TeleopTurtle();
+  void keyLoop();
+
+private:
+
+  
+  ros::NodeHandle nh_;
+  double linear_, angular_, l_scale_, a_scale_;
+  ros::Publisher vel_pub_;
+  
+};
+
+TeleopTurtle::TeleopTurtle():
+  linear_(0),
+  angular_(0),
+  l_scale_(2.0),
+  a_scale_(2.0)
+{
+  nh_.param("scale_angular", a_scale_, a_scale_);
+  nh_.param("scale_linear", l_scale_, l_scale_);
+
+  vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
+}
+
+int kfd = 0;
+struct termios cooked, raw;
+
+void quit(int sig)
+{
+  tcsetattr(kfd, TCSANOW, &cooked);
+  ros::shutdown();
+  exit(0);
+}
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "teleop_turtle");
+  TeleopTurtle teleop_turtle;
+
+  signal(SIGINT,quit);
+
+  teleop_turtle.keyLoop();
+  
+  return(0);
+}
+
+
+void TeleopTurtle::keyLoop()
+{
+  char c;
+  bool dirty=false;
+
+
+  // get the console in raw mode                                                              
+  tcgetattr(kfd, &cooked);
+  memcpy(&raw, &cooked, sizeof(struct termios));
+  raw.c_lflag &=~ (ICANON | ECHO);
+  // Setting a new line, then end of file                         
+  raw.c_cc[VEOL] = 1;
+  raw.c_cc[VEOF] = 2;
+  tcsetattr(kfd, TCSANOW, &raw);
+
+  puts("Reading from keyboard");
+  puts("---------------------------");
+  puts("Use arrow keys to move the turtle.");
+
+
+  for(;;)
+  {
+    // get the next event from the keyboard  
+    if(read(kfd, &c, 1) < 0)
+    {
+      perror("read():");
+      exit(-1);
+    }
+
+    linear_=angular_=0;
+    ROS_DEBUG("value: 0x%02X\n", c);
+  
+    switch(c)
+    {
+      case KEYCODE_L:
+        ROS_DEBUG("LEFT");
+        angular_ = 1.0;
+        dirty = true;
+        break;
+      case KEYCODE_R:
+        ROS_DEBUG("RIGHT");
+        angular_ = -1.0;
+        dirty = true;
+        break;
+      case KEYCODE_U:
+        ROS_DEBUG("UP");
+        linear_ = 1.0;
+        dirty = true;
+        break;
+      case KEYCODE_D:
+        ROS_DEBUG("DOWN");
+        linear_ = -1.0;
+        dirty = true;
+        break;
+    }
+   
+
+    turtlesim::Velocity vel;
+    vel.angular = a_scale_*angular_;
+    vel.linear = l_scale_*linear_;
+    if(dirty ==true)
+    {
+      vel_pub_.publish(vel);    
+      dirty=false;
+    }
+  }
+
+
+  return;
+}
+
+
+
