teleoperating problem [closed]

edit

Hallo! I want to teleoperate my robot with keyboard. So i tried to write a publisher, which supposed to publish commands. For this i used pr2_arm_teleoperating example. But the node does not seems to publish anything.

I created msg folder and Drive.msg file which looks like this:

int64 Acceleration
float64 Angle

And here is the publisher source code:

//#include "iostream.h"
  #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:
    cmd.Acceleration = cmd.Acceleration + 10;
    dirty = true;
    break;
  case KEYCODE_S:
    cmd.Acceleration = cmd.Acceleration - 10;
    dirty = true;
    break;
  case KEYCODE_A:
    cmd.Angle = cmd.Angle - 0.1;
    dirty = true;
    break;
  case KEYCODE_D:
    cmd.Angle = cmd.Angle + 0.1;
    dirty = true;
    break; 
  }


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


}
}

When i run my node and press 'wasd' buttons nothing happens. What is wrong? Any help and tips really appreciated.