Revision 60a90290

View differences:

scout/motors/msg/set_motors.msg
5 5
bool fr_set
6 6
bool bl_set
7 7
bool br_set
8
bool teleop_ON
8 9

  
9 10
# The absolute motor speeds to set (-128 to 127)
10 11
# TODO check that this is the correct range
scout/scoutsim/src/old_scout_teleop.cpp
1
#include <ros/ros.h>
2
#include <motors/set_motors.h>
3
#include <signal.h>
4
#include <termios.h>
5
#include <stdio.h>
6

  
7
// TODO: add this to a constants.h
8
#define QUEUE_SIZE 10
9

  
10
#define KEYCODE_R 0x43 
11
#define KEYCODE_L 0x44
12
#define KEYCODE_U 0x41
13
#define KEYCODE_D 0x42
14
#define KEYCODE_Q 0x71
15
#define KEYCODE_S 0x73
16

  
17
class TeleopScout
18
{
19
public:
20
    TeleopScout();
21
    void keyLoop();
22

  
23
private:
24
    ros::NodeHandle nh_;
25
    bool fl_set_, fr_set_, bl_set_, br_set_;
26
    int fl_speed_, fr_speed_, bl_speed_, br_speed_;
27
    char last_key;
28
    ros::Publisher motors_pub_;
29
};
30

  
31
TeleopScout::TeleopScout()
32
{
33
    fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
34
    fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
35
    motors_pub_ = nh_.advertise<motors::set_motors>(
36
                    "scout1/set_motors", QUEUE_SIZE);
37
}
38

  
39
int kfd = 0;
40
struct termios cooked, raw;
41

  
42
void quit(int sig)
43
{
44
    tcsetattr(kfd, TCSANOW, &cooked);
45
    ros::shutdown();
46
    exit(0);
47
}
48

  
49

  
50
int main(int argc, char** argv)
51
{
52
  ros::init(argc, argv, "teleop_scout");
53
  TeleopScout teleop_scout;
54

  
55
  signal(SIGINT,quit);
56

  
57
  teleop_scout.keyLoop();
58
  
59
  return(0);
60
}
61

  
62

  
63
void TeleopScout::keyLoop()
64
{
65
  char c;
66
  bool dirty=false;
67

  
68
  // get the console in raw mode                                                              
69
  tcgetattr(kfd, &cooked);
70
  memcpy(&raw, &cooked, sizeof(struct termios));
71
  raw.c_lflag &=~ (ICANON | ECHO);
72
  // Setting a new line, then end of file                         
73
  raw.c_cc[VEOL] = 1;
74
  raw.c_cc[VEOF] = 2;
75
  tcsetattr(kfd, TCSANOW, &raw);
76

  
77
  puts("Reading from keyboard");
78
  puts("---------------------------");
79
  puts("Use arrow keys to move the scout.");
80

  
81

  
82
  for(;;)
83
  {
84
    ROS_INFO("loop pass engaged");
85
    // get the next event from the keyboard  
86
    if(read(kfd, &c, 1) < 0)
87
    {
88
      perror("read():");
89
      exit(-1);
90
    }
91

  
92
    //ROS_DEBUG("value: 0x%02X\n", c);
93
    //ROS_INFO("value: 0x%02X\n", c);
94
    switch(c)
95
    {
96
      case KEYCODE_L:
97
        ROS_INFO("LEFT");
98
        fl_speed_ = bl_speed_ = 200;
99
        fr_speed_ = br_speed_ = -200;
100
        dirty = true;
101
        break;
102
      case KEYCODE_R:
103
        ROS_INFO("RIGHT");
104
        fl_speed_ = bl_speed_ = -200;
105
        fr_speed_ = br_speed_ = 200;
106
        dirty = true;
107
        break;
108
      case KEYCODE_U:
109
        ROS_INFO("UP");
110
        fl_speed_ = bl_speed_ = 200;
111
        fr_speed_ = br_speed_ = 200;
112
        dirty = true;
113
        break;
114
      case KEYCODE_D:
115
        ROS_INFO("DOWN");
116
        fl_speed_ = bl_speed_ = -200;
117
        fr_speed_ = br_speed_ = -200;
118
        dirty = true;
119
        break;
120
      default:
121
        ROS_INFO("test");
122
        fl_speed_ = bl_speed_ = 0;
123
        fr_speed_ = br_speed_ = 0;
124
        dirty = true;
125
        break;
126
    }
127
   
128
    motors::set_motors msg;
129
    msg.fl_set = fl_set_;
130
    msg.fr_set = fr_set_;
131
    msg.bl_set = bl_set_;
132
    msg.br_set = br_set_;
133
    msg.fl_speed = fl_speed_;
134
    msg.fr_speed = fr_speed_;
135
    msg.bl_speed = bl_speed_;
136
    msg.br_speed = br_speed_;
137
    
138
    if(dirty == true)
139
    {
140
      if (last_key != c)
141
      {
142
        motors_pub_.publish(msg);
143
      }
144
      dirty=false;
145
    }
146
    last_key = c;
147
  }
148

  
149
  return;
150
}
151

  
152

  
153

  
scout/scoutsim/src/scout.cpp
65 65
        : path_bitmap(path_bitmap)
66 66
          , sonar_visual_on(false)
67 67
          , sonar_on(true)
68
          , ignore_behavior(false)
69
          , current_teleop_scout("")
68 70
          , node (nh)
69 71
          , scout_image(scout_image)
70 72
          , pos(pos)
......
134 136
    {
135 137
        last_command_time = ros::WallTime::now();
136 138

  
137
        if(msg->fl_set)
139
        //ignore non-teleop commands if commands if teleop is ON
140
        if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
138 141
        {
139
            motor_fl_speed = absolute_to_mps(msg->fl_speed);
140
        }
141
        if(msg->fr_set)
142
        {
143
            motor_fr_speed = absolute_to_mps(msg->fr_speed);
144
        }
145
        if(msg->bl_set)
146
        {
147
            motor_bl_speed = absolute_to_mps(msg->bl_speed);
148
        }
149
        if(msg->br_set)
150
        {
151
            motor_br_speed = absolute_to_mps(msg->br_speed);
142
            if(msg->fl_set)
143
            {
144
                motor_fl_speed = absolute_to_mps(msg->fl_speed);
145
            }
146
            if(msg->fr_set)
147
            {
148
                motor_fr_speed = absolute_to_mps(msg->fr_speed);
149
            }
150
            if(msg->bl_set)
151
            {
152
                motor_bl_speed = absolute_to_mps(msg->bl_speed);
153
            }
154
            if(msg->br_set)
155
            {
156
                motor_br_speed = absolute_to_mps(msg->br_speed);
157
            }
152 158
        }
159

  
153 160
    }
154 161

  
155 162
    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
......
397 404
                                        const wxImage& walls_image,
398 405
                                        wxColour background_color,
399 406
                                        wxColour sonar_color,
400
                                        world_state state)
407
                                        world_state state, 
408
                                        string teleop_scoutname)
401 409
    {
402 410
        // Assume that the two motors on the same side will be set to
403 411
        // roughly the same speed. Does not account for slip conditions
......
409 417
        float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
410 418
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed);
411 419

  
420
        //store currently teleop'd scoutname
421
        std::stringstream ss;
422
        ss << "/" << teleop_scoutname;
423
        current_teleop_scout = ss.str();
424

  
412 425
        Vector2 old_pos = pos;
413 426

  
414 427
        // Update encoders
......
439 452
        int canvas_x = pos.x * PIX_PER_METER;
440 453
        int canvas_y = pos.y * PIX_PER_METER;
441 454

  
455

  
442 456
        {
443 457
            wxImage rotated_image =
444 458
                scout_image.Rotate(orient - PI/2.0,
scout/scoutsim/src/scout.h
118 118
                                         const wxImage& walls_image,
119 119
                                         wxColour background_color,
120 120
                                         wxColour sonar_color,
121
                                         world_state state);
121
                                         world_state state,std::string teleop_scoutname);
122 122
            void paint(wxDC& dc);
123 123
            void set_sonar_visual(bool on);
124 124

  
......
155 155
	        wxBitmap *path_bitmap;
156 156
	        bool sonar_visual_on;
157 157
            bool sonar_on;
158
            bool ignore_behavior;
159
            
160
            std::string current_teleop_scout;
158 161

  
159 162
            ros::NodeHandle node;
160 163

  
scout/scoutsim/src/sim_frame.cpp
130 130
        teleop_type = TELEOP_OFF;
131 131
        teleop_l_speed = 0;
132 132
        teleop_r_speed = 0;
133
        teleop_scoutname = "scout1";
134

  
133 135
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
134 136

  
135 137
        ROS_INFO("Starting scoutsim with node name %s",
......
240 242
        std::stringstream ss;
241 243
        ss << "/" << req.scout_name << "/set_motors";
242 244
        teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
245
        teleop_scoutname = req.scout_name.c_str();
246
        ROS_INFO("Teleoping %s...",teleop_scoutname.c_str()); //debug statement
243 247

  
244 248
        return true;
245 249
    }
......
533 537
        msg.fr_set = true;
534 538
        msg.bl_set = true;
535 539
        msg.br_set = true;
540
        msg.teleop_ON = true;
536 541

  
537 542
        msg.fl_speed = teleop_l_speed;
538 543
        msg.fr_speed = teleop_r_speed;
......
544 549

  
545 550
    void SimFrame::updateScouts()
546 551
    {
552
        std::string teleop_out = "";
553

  
547 554
        if (last_scout_update.isZero())
548 555
        {
549 556
            last_scout_update = ros::WallTime::now();
......
562 569

  
563 570
        for (; it != end; ++it)
564 571
        {
572

  
573
        //default to scout1
574
        if (teleop_type != TELEOP_OFF)
575
        {
576
            teleop_out = teleop_scoutname;
577
        }
578

  
565 579
            it->second->update(SIM_TIME_REFRESH_RATE,
566 580
                               path_dc, sonar_dc,
567 581
                               path_image, lines_image, walls_image,
568 582
                               path_dc.GetBackground().GetColour(),
569 583
                               sonar_dc.GetBackground().GetColour(),
570
                               state);
584
                               state,teleop_out);
571 585
        }
572 586

  
573 587
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
scout/scoutsim/src/sim_frame.h
148 148
            short teleop_l_speed;
149 149
            short teleop_r_speed;
150 150
            ros::Publisher teleop_pub;
151
            std::string teleop_scoutname;
151 152
            int teleop_type;
152 153

  
153 154
            int teleop_fluid_speed;

Also available in: Unified diff