Colony » Colony Scout » Colony Scout OS

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

}