Colony » Colony Scout

    Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), 

                            MAKE_EDGE(IRIGHT, 9, 40),

                            MAKE_EDGE(IUTURN, DEADEND, 0)};

    Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10),

                            MAKE_EDGE(IRIGHT, 10, 40),

                            MAKE_EDGE(IUTURN, 5, 10)};

    Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10),

                            MAKE_EDGE(IRIGHT, 11, 40),

                            MAKE_EDGE(IUTURN, 6, 10)};

    Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),

                            MAKE_EDGE(IRIGHT, 12, 40),

                            MAKE_EDGE(IUTURN, 7, 10)};

    Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),

                            MAKE_EDGE(ILEFT, 9, 40),

                            MAKE_EDGE(IUTURN, 2, 10)};

    Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10),

                            MAKE_EDGE(ILEFT, 10, 40),

                            MAKE_EDGE(IUTURN, 3, 10)};

    Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10),

                            MAKE_EDGE(ILEFT, 11, 40),

                            MAKE_EDGE(IUTURN, 4, 10)};

    Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10),

                            MAKE_EDGE(ILEFT, 12, 40),

                            MAKE_EDGE(IUTURN, DEADEND, 0)};

    Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10),

                            MAKE_EDGE(ILEFT, DEADEND, 0),

                            MAKE_EDGE(IUTURN, 9, 40)};

    Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10),

                            MAKE_EDGE(ILEFT, 5, 10),

                            MAKE_EDGE(IUTURN, 10, 40)};

    Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10),

                            MAKE_EDGE(ILEFT, 6, 10),

                            MAKE_EDGE(IUTURN, 11, 40)};

    Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),

                            MAKE_EDGE(ILEFT, 7, 10),

                            MAKE_EDGE(IUTURN, 12, 40)};

      Edge* e = map.pop_back();

      delete e;

    queue<State> q;

    q.push(curr_state);

    // not zero = visited, zero = unvisited, negative = start state

    visited[curr_state] = -1;

    while (!q.is_empty)

        State state = q.pop();

            Turn moves_list[MAX_NODES];

            int j = 0; // counter

            for(State child = state; visited[child] >= 0; 

                    child = visited[child]) //while not start state

                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;

                    }

                }

                // set visited to the parent of the new state

                q.push(new_state);

    //oops, no way to get to target from state

    return NULL;

#include <queue>

#include "lineDrive.h" // Get turn Macros

rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)

#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;

}

#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;

}