Revision 321961ca

View differences:

scout/scoutsim/CMakeLists.txt
37 37
#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
38 38
#rosbuild_add_executable(mimic tutorials/mimic.cpp)
39 39
rosbuild_add_executable(sonar_viz src/sonar_viz.cpp)
40
rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)
41
rosbuild_add_executable(scout_teleop_userinput src/scout_teleop_userinput.cpp)
scout/scoutsim/msg/teleop_input.msg
1
Header header
2
char last_key
scout/scoutsim/src/scout_teleop.cpp
1
#include "scout_teleop.h"
2

  
3
using namespace std;
4

  
5
TeleopScout::TeleopScout(string scoutname)
6
{
7
    fl_set = fr_set = bl_set = br_set = true;
8
    fl_speed = fr_speed = bl_speed = br_speed = 0;
9
    motors_pub = node.advertise<motors::set_motors>(
10
                    scoutname+"/set_motors", QUEUE_SIZE);
11
    key_input_sub = node.subscribe(scoutname+"/teleop_input",
12
                                        QUEUE_SIZE,
13
                                        &TeleopScout::teleop_input_callback,
14
                                        this);
15
}
16

  
17
void TeleopScout::teleop_input_callback(
18
            const scoutsim::teleop_input::ConstPtr& msg)
19
{
20
    last_key = msg->last_key;
21
    last_time = msg->header.stamp;
22
    //ROS_INFO("received key: 0x%02X ", last_key);
23
    //ROS_INFO("at last time: %f\n", last_time.toSec()); 
24
}
25

  
26
void TeleopScout::keyListenerLoop()
27
{	
28
    ROS_INFO("waiting for userinput commands...\n");
29
    ROS_INFO("current max wait time is:  %f\n", MAX_WAIT_TIME.toSec());
30
    while(ros::ok())
31
    {
32
	// if we haven't passed the waiting threshold
33
        if(last_time + MAX_WAIT_TIME > ros::Time::now())
34
        {
35
            switch(last_key)
36
            {
37
		// TODO: for some reason negative speeds go forward, not backward
38
		
39
                case KEYCODE_L:
40
                    //ROS_INFO("LEFT");
41
                    fl_speed = bl_speed = 200;
42
                    fr_speed = br_speed = -200;
43
                    break;
44
                case KEYCODE_R:
45
                    //ROS_INFO("RIGHT");
46
                    fl_speed = bl_speed = -200;
47
                    fr_speed = br_speed = 200;
48
                    break;
49
                case KEYCODE_U:
50
                    //ROS_INFO("UP");
51
                    fl_speed = bl_speed = -200;
52
                    fr_speed = br_speed = -200;
53
                    break;
54
                case KEYCODE_D:
55
                    //ROS_INFO("DOWN");
56
                    fl_speed = bl_speed = 200;
57
                    fr_speed = br_speed = 200;
58
                    break;
59
                default:
60
                    //don't publish a message yet; no change to key, 
61
                    // and no timeout yet
62
                    //ROS_INFO("no time out yet\n");
63
                    ros::spinOnce();
64
                    continue;
65
                    break;
66
            }
67
        }
68
        else if (fl_speed != 0 || fr_speed != 0 || 
69
                 bl_speed != 0 || br_speed != 0)
70
        {
71
	    ROS_INFO("no key press; stopping movement");
72
            fl_speed = fr_speed = bl_speed = br_speed = 0;
73
        }
74

  
75
        //send command to the motors
76
        motors::set_motors msg;
77
        msg.fl_set = fl_set;
78
        msg.fr_set = fr_set;
79
        msg.bl_set = bl_set;
80
        msg.br_set = br_set;
81
        msg.fl_speed = fl_speed;
82
        msg.fr_speed = fr_speed;
83
        msg.bl_speed = bl_speed;
84
        msg.br_speed = br_speed;
85
        motors_pub.publish(msg);
86
                
87
        ros::spinOnce();
88
    }
89
    return;
90
}
91

  
92
int main(int argc, char** argv)
93
{
94
    string scoutname = "";
95
    if(argc != 2)
96
    {
97
        cout << "Opens up a teleop process to send input to the motors" << endl;
98
        cout << "(Remember to create a scout_teleop_userinput process as well to actually capture keyboard input!)" << endl;
99
        cout << "Usage: " << argv[0] <<" <scoutname>" << endl;
100
        exit(0);
101
    }
102

  
103
    scoutname = argv[1];
104
    ros::init(argc, argv, scoutname+"_scout_teleop");
105
    TeleopScout teleop_scout = TeleopScout(scoutname);
106
    
107
    teleop_scout.keyListenerLoop();
108

  
109
    return(0);
110
}
scout/scoutsim/src/scout_teleop.h
1
#ifndef _SCOUT_TELEOP_
2
#define _SCOUT_TELEOP_
3

  
4
#include <ros/ros.h>
5
#include <motors/set_motors.h>
6
#include <signal.h>
7
#include <termios.h>
8
#include <stdio.h>
9

  
10
#include <scoutsim/teleop_input.h>
11

  
12
#define QUEUE_SIZE 10
13

  
14
#define KEYCODE_R 0x43 
15
#define KEYCODE_L 0x44
16
#define KEYCODE_U 0x41
17
#define KEYCODE_D 0x42
18
#define KEYCODE_Q 0x71
19
#define KEYCODE_S 0x73
20

  
21
// a second
22
#define MAX_WAIT_TIME ros::Duration(0.5)
23

  
24
class TeleopScout
25
{
26
    public:
27
        TeleopScout(std::string scoutname);
28
        void keyListenerLoop();
29

  
30
    private:
31
        ros::NodeHandle node;
32
        bool fl_set, fr_set, bl_set, br_set;
33
        int fl_speed, fr_speed, bl_speed, br_speed;
34
        char last_key;
35
        ros::Time last_time;
36
        void teleop_input_callback(const scoutsim::teleop_input::ConstPtr& msg);
37

  
38
        ros::Publisher motors_pub;
39
        ros::Subscriber key_input_sub;
40
};
41

  
42
#endif
scout/scoutsim/src/scout_teleop_userinput.cpp
1
#include "scout_teleop_userinput.h"
2

  
3
using namespace std;
4

  
5
TeleopInput::TeleopInput(string scoutname)
6
{
7
    input_pub = node.advertise<scoutsim::teleop_input>(
8
                   scoutname+"/teleop_input", QUEUE_SIZE);
9
    kfd = 0;
10

  
11
    // get the console in raw mode
12
    tcgetattr(kfd, &cooked);
13
    memcpy(&raw, &cooked, sizeof(struct termios));
14
    raw.c_lflag &=~ (ICANON | ECHO);
15
    // Setting a new line, then end of file                         
16
    raw.c_cc[VEOL] = 1;
17
    raw.c_cc[VEOF] = 2;
18
    tcsetattr(kfd, TCSANOW, &raw);
19

  
20
    //TeleopInput::keyLoop();
21
    return;
22
}
23

  
24
TeleopInput::~TeleopInput()
25
{
26
    // reset console mode to no longer be raw
27
    tcsetattr(kfd, TCSANOW, &cooked);
28
    ros::shutdown();
29
    exit(0);
30
}
31

  
32
void TeleopInput::keyLoop()
33
{
34
    ROS_INFO("press keys to start moving!");
35
    char pressed_key;
36

  
37
    while (ros::ok())
38
    {
39
        // get the next event from the keyboard  
40
        if(read(kfd, &pressed_key, 1) < 0)
41
        {
42
          perror("read():");
43
          exit(-1);
44
        }
45

  
46
	// note: printing this normally slows things the hell down
47
        ROS_DEBUG("pressed key 0x%02X at time: %f\n", pressed_key, ros::Time::now().toSec());
48

  
49
        scoutsim::teleop_input msg;
50
        msg.last_key = pressed_key;
51
	msg.header.stamp = ros::Time::now();
52
        input_pub.publish(msg);    
53
        
54
        ros::spinOnce();
55
    }
56
    return;
57
}
58

  
59
int main(int argc, char** argv)
60
{
61
    string scoutname = "";
62
    if(argc != 2)
63
    {
64
        cout << "Opens up a teleop process to read in user keyboard input" << endl;
65
	cout << "(Remember to have a scout_teleop process running simulatenously to actually process this input!)"<< endl;
66
        cout << "Usage: " << argv[0] <<" <scoutname>" << endl;
67
        exit(0);
68
    }
69
    
70
    scoutname = argv[1];
71
    ros::init(argc, argv, scoutname +"_scout_teleop_userinput");
72
    TeleopInput teleop_input = TeleopInput(scoutname);
73
    //signal(SIGINT,quit);
74
    teleop_input.keyLoop();
75
    
76
    return(0);
77
}
78

  
scout/scoutsim/src/scout_teleop_userinput.h
1
#ifndef _TELEOP_INPUT_
2
#define _TELEOP_INPUT_
3

  
4
#include <ros/ros.h>
5
#include <signal.h>
6
#include <termios.h>
7
#include <stdio.h>
8

  
9
#include <scoutsim/teleop_input.h>
10

  
11
#define QUEUE_SIZE 10
12

  
13
class TeleopInput{
14
    public:
15
        TeleopInput(std::string scoutname);
16
        ~TeleopInput();
17
        void keyLoop();
18
    private:
19
        int kfd;
20
        struct termios cooked, raw;
21
        ros::Publisher input_pub;
22
        ros::NodeHandle node;
23
};
24

  
25
#endif
26

  

Also available in: Unified diff