Colony » Colony Scout » Colony Scout OS

#include "scout_teleop_userinput.h"

using namespace std;

TeleopInput::TeleopInput(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);

    ros::shutdown();

    exit(0);

}

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)

{

    string scoutname = "";

    if(argc != 2)

    {

        cout << "Opens up a teleop process to read in user keyboard input" << endl;

        cout << "(Remember to have a scout_teleop process running simulatenously to actually process this input!)"<< endl;

        cout << "Usage: " << argv[0] <<" <scoutname>" << endl;

        exit(0);

    }

    scoutname = argv[1];

    ros::init(argc, argv, scoutname +"_scout_teleop_userinput");

    TeleopInput teleop_input = TeleopInput(scoutname);

    //signal(SIGINT,quit);

    teleop_input.keyLoop();

    return(0);

}