Revision 43811241

View differences:

scout/scoutsim/src/ghost_scout.cpp
67 67
      , start_y(pos.y)
68 68
      , orient(orient)
69 69
      , name(scoutname)
70
      , is_visible(false)
70 71
  {
71 72
    scout = wxBitmap(scout_image);
72 73

  
......
136 137

  
137 138
  void GhostScout::paint(wxDC& dc)
138 139
  {
140
      if (!is_visible)
141
      {
142
          return;
143
      }
144

  
139 145
    wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
140 146
    dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
141 147
        pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
142 148
  }
149

  
150
  string GhostScout::get_name()
151
  {
152
    return name;
153
  }
154

  
155
  void GhostScout::set_visible(bool vis)
156
  {
157
    is_visible = vis;
158
  }
143 159
}
scout/scoutsim/src/ghost_scout.h
84 84
      void paint(wxDC& dc);
85 85

  
86 86
      void setPosition(const ::messages::ScoutPosition& msg);
87
      std::string get_name();
88
      void set_visible(bool vis);
87 89

  
88 90
    private:
89 91
      unsigned int rgb_to_grey(unsigned char r,
......
104 106

  
105 107
      ros::Subscriber position;
106 108

  
109
      bool is_visible;
107 110
  };
108 111
}
109 112

  
scout/scoutsim/src/scout.cpp
63 63
                 wxBitmap *path_bitmap,
64 64
                 float orient)
65 65
        : path_bitmap(path_bitmap)
66
      , sonar_visual_on(sonar_visual_on)
67
      , node (nh)
66
          , sonar_visual_on(false)
67
          , node (nh)
68 68
          , scout_image(scout_image)
69 69
          , pos(pos)
70 70
          , orient(orient)
......
312 312
            }
313 313

  
314 314
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
315
            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
316
            if (isFront) //for some reason isFront = (!isFront) is not working
315
            sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2);
316
            if (isFront) // @todo for some reason isFront = (!isFront) is not working
317 317
            {
318 318
                isFront = FALSE;
319 319
            }
......
321 321
            {
322 322
                isFront = TRUE; 
323 323
            }
324
                
325 324
        }
326 325

  
327 326
        return d;
......
339 338
        last_sonar_time = ros::Time::now();
340 339

  
341 340
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
342
                                           sonar_position,sonar_dc);
341
                                           sonar_position, sonar_dc);
343 342
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
344
                                          sonar_position + 24,sonar_dc);
343
                                          sonar_position + 24, sonar_dc);
345 344

  
346 345
        // Publish
347 346
        sonar::sonar_distance msg;
......
393 392
    /// TODO remove dt param
394 393
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
395 394
                                        wxMemoryDC& sonar_dc,
396
                                        bool sonar_visual, 
397 395
                                        const wxImage& path_image,
398 396
                                        const wxImage& lines_image,
399 397
                                        const wxImage& walls_image,
......
401 399
                                        wxColour sonar_color,
402 400
                                        world_state state)
403 401
    {
404
        sonar_visual_on = sonar_visual;
405

  
406 402
        // Assume that the two motors on the same side will be set to
407 403
        // roughly the same speed. Does not account for slip conditions
408 404
        // when they are set to different speeds.
......
483 479
                         p.theta,sonar_dc);
484 480

  
485 481
        }
482

  
486 483
        // Figure out (and publish) the color underneath the scout
487 484
        {
488 485
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
......
521 518
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
522 519
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
523 520
    }
521

  
522
    void Scout::set_sonar_visual(bool on)
523
    {
524
        /// @todo Remove
525
        ROS_INFO("Sonar visual on set.");
526
        sonar_visual_on = on;
527
    }
524 528
}
scout/scoutsim/src/scout.h
112 112
                  const Vector2& pos, wxBitmap *path_bitmap, float orient);
113 113

  
114 114
            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
115
					 wxMemoryDC& sonar_dc,
116
                     bool sonar_visual,
115
                                         wxMemoryDC& sonar_dc,
117 116
                                         const wxImage& path_image,
118 117
                                         const wxImage& lines_image,
119 118
                                         const wxImage& walls_image,
120 119
                                         wxColour background_color,
121
         				 wxColour sonar_color,
120
                                         wxColour sonar_color,
122 121
                                         world_state state);
123 122
            void paint(wxDC& dc);
123
            void set_sonar_visual(bool on);
124 124

  
125 125
        private:
126 126
            float absolute_to_mps(int absolute_speed);
scout/scoutsim/src/scoutsim.cpp
137 137
    EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop)
138 138
    EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise)
139 139
    EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid)
140
    EVT_MENU(ID_SONAR, scoutsim::SimFrame::showSonar)
141
    EVT_MENU(ID_TEST, scoutsim::SimFrame::doTest)
142 140
END_EVENT_TABLE()
143 141

  
144 142
DECLARE_APP(ScoutApp);
scout/scoutsim/src/sim_frame.cpp
88 88
        wxBitmap lines_bitmap;
89 89
        wxBitmap walls_bitmap;
90 90
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
91
        sonar_visual_on = true;
92 91

  
93 92
        // Try to load the file; if it fails, make a new blank file
94 93
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
......
113 112
        spawn_srv = nh.advertiseService("spawn",
114 113
                                        &SimFrame::spawnCallback, this);
115 114
        kill_srv = nh.advertiseService("kill",
116
                                       &SimFrame::killCallback, this);
115
                                        &SimFrame::killCallback, this);
116
        set_sonar_viz_srv = nh.advertiseService("set_sonar_viz",
117
                                        &SimFrame::setSonarVizCallback, this);
118
        set_ghost_srv = nh.advertiseService("set_ghost",
119
                                        &SimFrame::setGhostCallback, this);
120
        set_teleop_srv = nh.advertiseService("set_teleop",
121
                                        &SimFrame::setTeleopCallback, this);
117 122

  
118 123
        // Subscribe and publisher wirless from robots
119 124
        wireless_receive = nh.advertise< ::messages::WirelessPacket>(
......
122 127
            &SimFrame::wirelessCallback, this);
123 128

  
124 129
        // Teleop
125
        teleop_type = TELEOP_PRECISE;
130
        teleop_type = TELEOP_OFF;
126 131
        teleop_l_speed = 0;
127 132
        teleop_r_speed = 0;
128 133
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);
......
136 141
        menuFile->Append(ID_QUIT, _("E&xit"));
137 142

  
138 143
        wxMenu *menuSim = new wxMenu;
139
        menuSim->Append(ID_SONAR, _("S&onar"));
140 144
        menuSim->Append(ID_CLEAR, _("&Clear"));
141 145

  
142 146
        wxMenu *menuView = new wxMenu;
......
149 153
        menuTeleop->Append(ID_TELEOP_PRECISE, _("&Precise"));
150 154
        menuTeleop->Append(ID_TELEOP_FLUID, _("&Fluid"));
151 155

  
152
        wxMenu *menuTest = new wxMenu;
153
        menuTest->Append(ID_TEST, _("&Scouts go here"));
154

  
155 156
        wxMenuBar *menuBar = new wxMenuBar;
156 157
        menuBar->Append(menuFile, _("&File"));
157 158
        menuBar->Append(menuSim, _("&Sim"));
158 159
        menuBar->Append(menuView, _("&View"));
159 160
        menuBar->Append(menuTeleop, _("&Teleop"));
160
        menuBar->Append(menuTest, _("T&est"));
161 161

  
162 162
        SetMenuBar(menuBar);
163 163

  
......
178 178
        std::string name = spawnScout(req.name, req.x, req.y, req.theta);
179 179
        if (name.empty())
180 180
        {
181
            ROS_ERROR("A scout named [%s] already exists", req.name.c_str());
181
            ROS_WARN("A scout named [%s] already exists", req.name.c_str());
182 182
            return false;
183 183
        }
184 184

  
185 185
        res.name = name;
186

  
187 186
        return true;
188 187
    }
189 188

  
......
193 192
        M_Scout::iterator it = scouts.find(req.name);
194 193
        if (it == scouts.end())
195 194
        {
196
            ROS_ERROR("Tried to kill scout [%s], which does not exist",
197
                      req.name.c_str());
195
            ROS_WARN("Tried to kill scout [%s], which does not exist",
196
                     req.name.c_str());
198 197
            return false;
199 198
        }
200 199

  
......
203 202
        return true;
204 203
    }
205 204

  
205
    bool SimFrame::setSonarVizCallback(SetSonarViz::Request& req,
206
                                       SetSonarViz::Response&)
207
    {
208
        M_Scout::iterator it = scouts.find(req.scout_name);
209
        if (it == scouts.end())
210
        {
211
            ROS_WARN("Tried to set sonar on scout [%s], which does not exist",
212
                     req.scout_name.c_str());
213
            return false;
214
        }
215

  
216
        it->second->set_sonar_visual(req.on);
217
        return true;
218
    }
219

  
220
    bool SimFrame::setGhostCallback(SetGhost::Request& req,
221
                                    SetGhost::Response&)
222
    {
223
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
224
        {
225
            if (ghost_scouts.at(i)->get_name() == req.scout_name)
226
            {
227
                ghost_scouts.at(i)->set_visible(req.on);
228
                return true;
229
            }
230
        }
231

  
232
        ROS_WARN("Tried to set ghost on scout [%s], which does not exist",
233
                 req.scout_name.c_str());
234
        return false;
235
    }
236

  
237
    bool SimFrame::setTeleopCallback(SetTeleop::Request& req,
238
                                     SetTeleop::Response&)
239
    {
240
        std::stringstream ss;
241
        ss << "/" << req.scout_name << "/set_motors";
242
        teleop_pub = nh.advertise<motors::set_motors>(ss.str(), 1000);
243

  
244
        return true;
245
    }
246

  
206 247
    bool SimFrame::hasScout(const std::string& name)
207 248
    {
208 249
        return scouts.find(name) != scouts.end();
......
214 255
        std::string real_name = name;
215 256
        if (real_name.empty())
216 257
        {
258
            // Generate the name scoutX, where X is an increasing number.
217 259
            do
218 260
            {
219 261
                std::stringstream ss;
220 262
                ss << "scout" << ++id_counter;
221 263
                real_name = ss.str();
222
            } while (hasScout(real_name));
264
            }
265
            while (hasScout(real_name));
223 266
        }
224 267
        else
225 268
        {
......
280 323
        clear();
281 324
    }
282 325

  
283
    void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event))
284
    {
285
        sonar_visual_on = not sonar_visual_on;
286
        clear();
287
    }
288

  
289
    void SimFrame::doTest(wxCommandEvent& WXUNUSED(event))
290
    {
291
        clear();
292
    }
293

  
294 326
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
295 327
    {
296 328
        display_map_name = map_base_name;
......
353 385
        {
354 386
            it->second->paint(dc);
355 387
        }
356
        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
388
        for (unsigned int i=0; i < ghost_scouts.size(); ++i)
357 389
        {
358 390
            ghost_scouts.at(i)->paint(dc);
359 391
        }
......
410 442
        }
411 443
        else if (wxGetKeyState(WXK_LEFT))
412 444
        {
413
            teleop_l_speed = -TELEOP_PRECISE_SPEED * 4;
414
            teleop_r_speed = TELEOP_PRECISE_SPEED * 4;
445
            teleop_l_speed = -TELEOP_PRECISE_TURN_SPEED;
446
            teleop_r_speed = TELEOP_PRECISE_TURN_SPEED;
415 447
        }
416 448
        else if (wxGetKeyState(WXK_RIGHT))
417 449
        {
418
            teleop_l_speed = TELEOP_PRECISE_SPEED * 4;
419
            teleop_r_speed = -TELEOP_PRECISE_SPEED * 4;
450
            teleop_l_speed = TELEOP_PRECISE_TURN_SPEED;
451
            teleop_r_speed = -TELEOP_PRECISE_TURN_SPEED;
420 452
        }
421 453
    }
422 454

  
......
531 563
        for (; it != end; ++it)
532 564
        {
533 565
            it->second->update(SIM_TIME_REFRESH_RATE,
534
                               path_dc,sonar_dc,sonar_visual_on,
566
                               path_dc, sonar_dc,
535 567
                               path_image, lines_image, walls_image,
536 568
                               path_dc.GetBackground().GetColour(),
537 569
                               sonar_dc.GetBackground().GetColour(),
538 570
                               state);
539 571
        }
540 572

  
541
        for (unsigned int i=0; i<ghost_scouts.size(); ++i)
573
        for (unsigned int i = 0; i < ghost_scouts.size(); ++i)
542 574
        {
543 575
            ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
544 576
                path_dc.GetBackground().GetColour(), state);
scout/scoutsim/src/sim_frame.h
58 58
#include <std_srvs/Empty.h>
59 59
#include <scoutsim/Spawn.h>
60 60
#include <scoutsim/Kill.h>
61
#include <scoutsim/SetSonarViz.h>
62
#include <scoutsim/SetGhost.h>
63
#include <scoutsim/SetTeleop.h>
61 64
#include <motors/set_motors.h>
62 65
#include <map>
63 66

  
......
76 79
#define ID_TELEOP_NONE 7
77 80
#define ID_TELEOP_PRECISE 8
78 81
#define ID_TELEOP_FLUID 9
79
#define ID_SONAR 10
80
#define ID_TEST 11
81 82

  
82 83
// Absolute speeds (-128 - 127)
83
#define TELEOP_PRECISE_SPEED 50
84
/// @todo: Clean this up a little; we should be risking overflowing shorts.
85
#define TELEOP_PRECISE_SPEED 60
86
#define TELEOP_PRECISE_TURN_SPEED 124
84 87
#define TELEOP_FLUID_MAX_SPEED 100
85 88
#define TELEOP_FLUID_INC 8
86 89

  
......
109 112
            void stopTeleop(wxCommandEvent& event);
110 113
            void startTeleopPrecise(wxCommandEvent& event);
111 114
            void startTeleopFluid(wxCommandEvent& event);
112
            void showSonar(wxCommandEvent& event);
113
            void doTest(wxCommandEvent& event);
114 115

  
115 116
            DECLARE_EVENT_TABLE()
116 117

  
......
134 135
            bool spawnCallback(Spawn::Request&, Spawn::Response&);
135 136
            bool killCallback(Kill::Request&, Kill::Response&);
136 137

  
138
            bool setSonarVizCallback(SetSonarViz::Request&, SetSonarViz::Response&);
139
            bool setGhostCallback(SetGhost::Request&, SetGhost::Response&);
140
            bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&);
141

  
137 142
            void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg);
138 143

  
139 144
            ros::Subscriber wireless_send;
......
165 170
            ros::ServiceServer reset_srv;
166 171
            ros::ServiceServer spawn_srv;
167 172
            ros::ServiceServer kill_srv;
173
            ros::ServiceServer set_sonar_viz_srv;
174
            ros::ServiceServer set_ghost_srv;
175
            ros::ServiceServer set_teleop_srv;
168 176

  
169 177
            typedef std::map<std::string, ScoutPtr> M_Scout;
170 178
            M_Scout scouts;
......
176 184
            float width_in_meters;
177 185
            float height_in_meters;
178 186

  
179
            bool sonar_visual_on;
180

  
181 187
            std::string map_base_name;
182 188
            std::string map_lines_name;
183 189
            std::string map_walls_name;
scout/scoutsim/srv/Spawn.srv
3 3
float32 theta
4 4
string name # Optional.  A unique name will be created and returned if this is empty
5 5
---
6
string name
6
string name

Also available in: Unified diff