Teleop based control of a custom vehicle

edit

Here is my implementation of teleop for carlike robot. Instead of Twist (whish has 6 variables) I use simple Drive message containing only acceleration and steering angle. Hope it would be helpful somehow.

    #include "ros/ros.h"
#include "drive_base/Drive.h"
#include "sstream"
#include <termios.h>
#include <signal.h>
#include <fcntl.h>


  #define KEYCODE_A 0x61
  #define KEYCODE_D 0x64
  #define KEYCODE_S 0x73
  #define KEYCODE_W 0x77

  class TeleopRoboCVKeyboard
  {
    private:
    drive_base::Drive cmd;

    ros::NodeHandle n_;
    ros::Publisher drive_pub_;

    public:
    void init()
    {


      //Clear out our cmd - these values are roundabout initials

      cmd.Acceleration = 0;
      cmd.Angle = 0;

      drive_pub_ = n_.advertise<drive_base::Drive>("command", 100);

      ros::NodeHandle n_private("~");
    }

    ~TeleopRoboCVKeyboard()   { }
    void keyboardLoop();
  };

  int kfd = 0;
  struct termios cooked, raw;

  void quit(int sig)
  {
    tcsetattr(kfd, TCSANOW, &cooked);
   exit(0);
  }

  int main(int argc, char** argv)
  {
    ros::init(argc, argv, "drive_cmd_publisher");

    TeleopRoboCVKeyboard tpk;
    tpk.init();

    signal(SIGINT,quit);

    tpk.keyboardLoop();

    return(0);
  }

  void TeleopRoboCVKeyboard::keyboardLoop()
  {
    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 'WS' to forward/back");
    puts("Use 'AD' to left/right");

    for(;;)
    {
      // get the next event from the keyboard
      if(read(kfd, &c, 1) < 0)
      {
        perror("read():");
        exit(-1);
      }

      switch(c)
      {
        // Walking
      case KEYCODE_W:
    puts("throttle");
        cmd.Acceleration = cmd.Acceleration + 10;
        dirty = true;
        break;
      case KEYCODE_S:
    puts("brake");
        cmd.Acceleration = cmd.Acceleration - 10;
        dirty = true;
        break;
      case KEYCODE_A:
    puts("turn left");
        cmd.Angle = cmd.Angle - 0.1;
        dirty = true;
        break;
      case KEYCODE_D:
    puts("turn right");
        cmd.Angle = cmd.Angle + 0.1;
        dirty = true;
        break; 
      }


      if (dirty == true)
      {
        drive_pub_.publish(cmd);
      }


    }
  }