Colony » Colony Scout » Colony Scout OS

rosbuild_add_executable(scout_teleop_userinput src/scout_teleop_userinput.cpp)

      msg/Color.msg  msg/Pose.msg  msg/Velocity.msg msg/teleop_input.msg

#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 "scout_teleop.h"

TeleopScout::TeleopScout(std::string scoutname)

    fl_set = fr_set = bl_set = br_set = true;

    fl_speed = fr_speed = bl_speed = br_speed = 0;

    motors_pub = node.advertise<motors::set_motors>(

                    scoutname+"/set_motors", QUEUE_SIZE);

    key_input_sub = node.subscribe(scoutname+"/teleop_input",

                                        QUEUE_SIZE,

                                        &TeleopScout::teleop_input_callback,

                                        this);

void TeleopScout::teleop_input_callback(

            const scoutsim::teleop_input::ConstPtr& msg)

    last_key = msg->last_key;

    last_time = msg->header.stamp;

    ROS_INFO("received key: 0x%02X ", last_key);

    ROS_INFO("at last time: %f\n", last_time.toSec()); 

void TeleopScout::keyListenerLoop()

    while(ros::ok())

        if(last_time + MAX_WAIT_TIME < ros::Time::now())

        {

            switch(last_key)

            {

                case KEYCODE_L:

                    //ROS_INFO("LEFT");

                    fl_speed = bl_speed = 200;

                    fr_speed = br_speed = -200;

                    break;

                case KEYCODE_R:

                    //ROS_INFO("RIGHT");

                    fl_speed = bl_speed = -200;

                    fr_speed = br_speed = 200;

                    break;

                case KEYCODE_U:

                    //ROS_INFO("UP");

                    fl_speed = bl_speed = 200;

                    fr_speed = br_speed = 200;

                    break;

                case KEYCODE_D:

                    //ROS_INFO("DOWN");

                    fl_speed = bl_speed = -200;

                    fr_speed = br_speed = -200;

                    break;

                default:

                    //don't publish a message yet; no change to key, 

                    // and no timeout yet

                    ROS_INFO("no time out yet\n");

                    ros::spinOnce();

                    continue;

                    break;

            }

        }

        else if (fl_speed != 0 || fr_speed != 0 || 

                 bl_speed != 0 || br_speed != 0)

        {

            fl_speed = fr_speed = bl_speed = br_speed = 0;

        }

        //send command to the motors

        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;

        motors_pub.publish(msg);

        ros::spinOnce();

    return;

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "scout_teleop");

    TeleopScout teleop_scout = TeleopScout("scout1");

    teleop_scout.keyListenerLoop();

    return(0);