Colony » Colony Scout » Colony Scout OS

Header header

char last_key

#ifndef _SCOUT_TELEOP_

#define _SCOUT_TELEOP_

#include <ros/ros.h>

#include <motors/set_motors.h>

#include <signal.h>

#include <termios.h>

#include <stdio.h>

#include <scoutsim/teleop_input.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

// a second

#define MAX_WAIT_TIME ros::Duration(1, 0)

class TeleopScout

{

    public:

        TeleopScout(std::string scoutname);

        void keyListenerLoop();

    private:

        ros::NodeHandle node;

        bool fl_set, fr_set, bl_set, br_set;

        int fl_speed, fr_speed, bl_speed, br_speed;

        char last_key;

        ros::Time last_time;

        void teleop_input_callback(const scoutsim::teleop_input::ConstPtr& msg);

        ros::Publisher motors_pub;

        ros::Subscriber key_input_sub;

};

#endif

#include "scout_teleop_userinput.h"

TeleopInput::TeleopInput(std::string scoutname)

{

    input_pub = node.advertise<scoutsim::teleop_input>(scoutname+"/teleop_input", QUEUE_SIZE);

    kfd = 0;

    // 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);

    //TeleopInput::keyLoop();

    return;

}

TeleopInput::~TeleopInput()

{

    // reset console mode to no longer be raw

    tcsetattr(kfd, TCSANOW, &cooked);

    return;

}

void TeleopInput::keyLoop()

{

    ROS_INFO("press keys to start moving!");

    char pressed_key;

    while (ros::ok())

    {

        // get the next event from the keyboard  

        if(read(kfd, &pressed_key, 1) < 0)

        {

          perror("read():");

          exit(-1);

        }

	// note: printing this normally slows things the hell down

        ROS_DEBUG("pressed key 0x%02X at time: %f\n", pressed_key, ros::Time::now().toSec());

        scoutsim::teleop_input msg;

        msg.last_key = pressed_key;

	msg.header.stamp = ros::Time::now();

        input_pub.publish(msg);    

        ros::spinOnce();

    }

    return;

}

int main(int argc, char** argv)

{

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

    TeleopInput teleop_input = TeleopInput("scout1");

    teleop_input.keyLoop();

    return(0);

}

#ifndef _TELEOP_INPUT_

#define _TELEOP_INPUT_

#include <ros/ros.h>

#include <signal.h>

#include <termios.h>

#include <stdio.h>

#include <scoutsim/teleop_input.h>

#define QUEUE_SIZE 10

class TeleopInput{

    public:

        TeleopInput(std::string scoutname);

        ~TeleopInput();

        void keyLoop();

    private:

        int kfd;

        struct termios cooked, raw;

        ros::Publisher input_pub;

        ros::NodeHandle node;

};

#endif