Colony » Colony Scout » Colony Scout OS

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

}