keyboard_publisher.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <termios.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>

#include "ros/ros.h"

#include <sensor_msgs/Joy.h>




#define KEYCODE_N 0x6E  //run
#define KEYCODE_M 0x6D  //mode
#define KEYCODE_H 0x68  //help


//platform
#define KEYCODE_A 0x61
#define KEYCODE_D 0x64
#define KEYCODE_S 0x73
#define KEYCODE_W 0x77
#define KEYCODE_Q 0x71
#define KEYCODE_E 0x65


//arm+tray
#define KEYCODE_1 0x31
#define KEYCODE_2 0x32
#define KEYCODE_3 0x33
#define KEYCODE_4 0x34
#define KEYCODE_5 0x35
#define KEYCODE_6 0x36
#define KEYCODE_7 0x37
#define KEYCODE_8 0x38

//#define KEYCODE_Q 0x61
//#define KEYCODE_W 0x61
//#define KEYCODE_E 0x61
#define KEYCODE_R 0x72
#define KEYCODE_T 0x74
#define KEYCODE_Z 0x7A
#define KEYCODE_U 0x75
#define KEYCODE_I 0x69


//torso+head
//#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
#define KEYCODE_F 0x66
#define KEYCODE_G 0x67

#define KEYCODE_Y 0x79
#define KEYCODE_X 0x78
#define KEYCODE_C 0x63
#define KEYCODE_V 0x76
#define KEYCODE_B 0x62



const int PUBLISH_FREQ = 5.0;

int kfd = 0;
struct termios cooked, raw;

bool fast_toggle,mode;

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


void composeJoyMessage(sensor_msgs::Joy &msg, char c)
{
  msg.buttons[5]=0;

  if(fast_toggle)
    msg.buttons[7]=1;

  if(mode) //joint_mode
  {
    switch(c)
    {
    //positive
    case KEYCODE_1: //arm
      msg.buttons[0]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_2:
      msg.buttons[0]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_3:
      msg.buttons[1]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_4:
      msg.buttons[1]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_5:
      msg.buttons[2]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_6:
      msg.buttons[2]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_7:
      msg.buttons[3]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_8: //tray
      msg.buttons[3]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_Y: //torso
      msg.buttons[6]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_S:
      msg.buttons[6]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_C:
      msg.buttons[4]=1;
      msg.axes[4]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_F:
      msg.buttons[4]=1;
      msg.axes[5]=1.0;
      msg.buttons[5]=1;
      break;
    //case KEYCODE_G: //head
    //  msg.buttons[2]=1;
    //  msg.axes[4]=1.0;
    //  msg.buttons[5]=1;
    //  break;

    //negative
    case KEYCODE_Q: //arm
      msg.buttons[0]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_W:
      msg.buttons[0]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_E:
      msg.buttons[1]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_R:
      msg.buttons[1]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_T:
      msg.buttons[2]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_Z:
      msg.buttons[2]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_U:
      msg.buttons[3]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_I: //tray
      msg.buttons[3]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_A: //torso
      msg.buttons[6]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_X:
      msg.buttons[6]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_D:
      msg.buttons[4]=1;
      msg.axes[4]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_V:
      msg.buttons[4]=1;
      msg.axes[5]=-1.0;
      msg.buttons[5]=1;
      break;
    //case KEYCODE_B: //head
    //  msg.buttons[2]=1;
    //  msg.axes[4]=-1.0;
    //  msg.buttons[5]=1;
    //  break;
    }
  }
  else //platform_mode
  {
    switch(c)
    {
    case KEYCODE_W:
      msg.axes[1]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_S:
      msg.axes[1]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_A:
      msg.axes[0]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_D:
      msg.axes[0]=-1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_Q:
      msg.axes[2]=1.0;
      msg.buttons[5]=1;
      break;
    case KEYCODE_E:
      msg.axes[2]=-1.0;
      msg.buttons[5]=1;
      break;
    }
  }
}


void showHelp()
{
    puts("");
    puts("Reading from keyboard");
    puts("---------------------------");
    puts("Use 'm' to toggle modes (joint/platform)");
    puts("Use 'n' to toggle run");
    puts("---------------------------");
    puts("In platform_mode");
    puts("Use 'wasd' to translate");
    puts("Use 'qe' to yaw");
    puts("---------------------------");
    puts("In joint_mode");
    puts("Use '1'-'7' and 'q'-'u' for arm");
    puts("Use '8' and 'i' for tray");
    puts("Use 'a'-'f' and 'y'-'v' for torso");
    puts("---------------------------");
    puts("Use 'h' to show this help");
    puts("Hit 'SPACE' to stop movement");
    puts("");
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "keyboard_publisher");
  ros::NodeHandle n;

    puts("");
    puts("Reading from keyboard");
    puts("---------------------------");
    puts("Use 'm' to toggle modes (joint/platform)");
    puts("Use 'n' to toggle run");
    puts("---------------------------");
    puts("In platform_mode");
    puts("Use 'wasd' to translate");
    puts("Use 'qe' to yaw");
    puts("---------------------------");
    puts("In joint_mode");
    puts("Use '1'-'7' and 'q'-'u' for arm");
    puts("Use '8' and 'i' for tray");
    puts("Use 'a'-'f' and 'y'-'v' for torso");
    puts("---------------------------");
    puts("Use 'h' to show this help");
    puts("Hit 'SPACE' to stop movement");
    puts("");

  signal(SIGINT,quit);

  // 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;
  //raw.c_cc[VMIN] = 0;
  //raw.c_cc[VTIME] = 0;
  tcsetattr(kfd, TCSANOW, &raw);


  fast_toggle=false;
  mode=false;

  ros::Publisher keyboard_pub = n.advertise<sensor_msgs::Joy>("joy", 1);
  char c;

  while (ros::ok())
  {
    ros::spinOnce();

    sensor_msgs::Joy msg;
  msg.axes.resize(6);
  msg.buttons.resize(12);

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

  //ROS_INFO("I got key %d",c);

  switch(c)
    {
      // Fast_Toggle
    case KEYCODE_N:
    if(fast_toggle)
    {
      fast_toggle=false;
      ROS_INFO("fast_mode: OFF");
    }
    else
    {
      fast_toggle=true;
      ROS_INFO("fast_mode: ON");
    }
      break;
      // Mode_Toggle
    case KEYCODE_M:
      if(mode)
      {
        mode=false;
        ROS_INFO("Mode: Platform_Mode");
      }
      else
      {
        mode=true;
        ROS_INFO("Mode: Joint_Mode");
      }
    break;
  case KEYCODE_H:
    showHelp();
    break;
  }


  composeJoyMessage(msg, c);

    keyboard_pub.publish(msg);
  }


  return 0;
}