Revision 2eaafff2
ID | 2eaafff20f1014a2a7998573e21437aedd65eb45 |
Added maps and menus in scoutsim.
Menu options for about, quit, clear, and viewing different maps.
scout/libscout/src/BehaviorProcess.cpp | ||
---|---|---|
47 | 47 |
{ |
48 | 48 |
cout << "You have started this behavior in hardware mode." << endl |
49 | 49 |
<< "To start in software mode, use: " << argv[0] |
50 |
<< " <scoutname> <behavior#>" << endl; |
|
50 |
<< " <scoutname> <behavior#> <behaviorname>" << endl;
|
|
51 | 51 |
} |
52 | 52 |
else |
53 | 53 |
{ |
scout/scoutsim/src/scoutsim.cpp | ||
---|---|---|
52 | 52 |
|
53 | 53 |
#include <boost/thread.hpp> |
54 | 54 |
|
55 |
#include <stdio.h> |
|
56 |
|
|
55 | 57 |
#include "sim_frame.h" |
56 | 58 |
|
57 | 59 |
#ifdef __WXMAC__ |
58 | 60 |
#include <ApplicationServices/ApplicationServices.h> |
59 | 61 |
#endif |
60 | 62 |
|
63 |
using namespace std; |
|
64 |
|
|
61 | 65 |
class ScoutApp : public wxApp |
62 | 66 |
{ |
63 | 67 |
public: |
... | ... | |
70 | 74 |
|
71 | 75 |
bool OnInit() |
72 | 76 |
{ |
77 |
std::cout << "Starting scout app." << std::endl; |
|
73 | 78 |
#ifdef __WXMAC__ |
74 | 79 |
ProcessSerialNumber PSN; |
75 | 80 |
GetCurrentProcess(&PSN); |
76 | 81 |
TransformProcessType(&PSN,kProcessTransformToForegroundApplication); |
77 | 82 |
SetFrontProcess(&PSN); |
78 | 83 |
#endif |
79 |
|
|
80 | 84 |
// Create our own copy of argv, with regular char*s. |
81 | 85 |
local_argv = new char*[ argc ]; |
82 | 86 |
for ( int i = 0; i < argc; ++i ) |
... | ... | |
84 | 88 |
local_argv[ i ] = strdup( wxString( argv[ i ] ).mb_str() ); |
85 | 89 |
} |
86 | 90 |
|
91 |
// Check for incorrect usage |
|
92 |
if (argc != 2) |
|
93 |
{ |
|
94 |
cout << endl << "Error." << endl << endl; |
|
95 |
cout << "Usage: " << local_argv[0] << " <map name>" << endl; |
|
96 |
cout << "To use maps/example.bmp, use 'example'." << endl; |
|
97 |
exit(0); |
|
98 |
} |
|
99 |
|
|
100 |
std::cout << "About to init node" << std::endl; |
|
87 | 101 |
ros::init(argc, local_argv, "scoutsim"); |
102 |
std::cout << "About to reset." << std::endl; |
|
88 | 103 |
nh.reset(new ros::NodeHandle); |
89 | 104 |
|
105 |
std::cout << "About to init image handlers." << std::endl; |
|
90 | 106 |
wxInitAllImageHandlers(); |
91 | 107 |
|
92 |
scoutsim::SimFrame* frame = new scoutsim::SimFrame(NULL); |
|
108 |
std::cout << "About to make a sim frame." << std::endl; |
|
109 |
scoutsim::SimFrame* frame = new scoutsim::SimFrame(NULL, string(local_argv[1])); |
|
93 | 110 |
|
94 | 111 |
SetTopWindow(frame); |
95 | 112 |
frame->Show(); |
96 | 113 |
|
114 |
std::cout << "Constructor done!" << std::endl; |
|
97 | 115 |
return true; |
98 | 116 |
} |
99 | 117 |
|
... | ... | |
109 | 127 |
} |
110 | 128 |
}; |
111 | 129 |
|
130 |
BEGIN_EVENT_TABLE(scoutsim::SimFrame, wxFrame) |
|
131 |
EVT_MENU(ID_QUIT, scoutsim::SimFrame::onQuit) |
|
132 |
EVT_MENU(ID_ABOUT, scoutsim::SimFrame::onAbout) |
|
133 |
EVT_MENU(ID_CLEAR, scoutsim::SimFrame::onClear) |
|
134 |
EVT_MENU(ID_MAP, scoutsim::SimFrame::showMap) |
|
135 |
EVT_MENU(ID_LINES, scoutsim::SimFrame::showLines) |
|
136 |
END_EVENT_TABLE() |
|
137 |
|
|
112 | 138 |
DECLARE_APP(ScoutApp); |
113 | 139 |
IMPLEMENT_APP(ScoutApp); |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
47 | 47 |
|
48 | 48 |
#include "sim_frame.h" |
49 | 49 |
|
50 |
#include <stdio.h> |
|
51 |
|
|
50 | 52 |
#include <ros/package.h> |
51 | 53 |
#include <cstdlib> |
52 | 54 |
#include <ctime> |
53 | 55 |
|
54 |
#define DEFAULT_BG_R 0x45 |
|
55 |
#define DEFAULT_BG_G 0x56 |
|
56 |
#define DEFAULT_BG_B 0xff |
|
56 |
using namespace std; |
|
57 | 57 |
|
58 | 58 |
namespace scoutsim |
59 | 59 |
{ |
60 |
|
|
61 |
SimFrame::SimFrame(wxWindow* parent) |
|
60 |
SimFrame::SimFrame(wxWindow* parent, string map_name) |
|
62 | 61 |
: wxFrame(parent, wxID_ANY, wxT("ScoutSim"), wxDefaultPosition, |
63 | 62 |
wxSize(500, 500), wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER) |
64 | 63 |
, frame_count(0) |
65 | 64 |
, id_counter(0) |
66 | 65 |
{ |
66 |
std::cout << "Constructing sim frame." << std::endl; |
|
67 |
|
|
67 | 68 |
srand(time(NULL)); |
68 | 69 |
|
69 | 70 |
update_timer = new wxTimer(this); |
... | ... | |
74 | 75 |
Connect(wxEVT_PAINT, wxPaintEventHandler(SimFrame::onPaint), |
75 | 76 |
NULL, this); |
76 | 77 |
|
77 |
nh.setParam("background_r", DEFAULT_BG_R); |
|
78 |
nh.setParam("background_g", DEFAULT_BG_G); |
|
79 |
nh.setParam("background_b", DEFAULT_BG_B); |
|
80 |
|
|
81 | 78 |
std::string scouts[SCOUTSIM_NUM_SCOUTS] = |
82 | 79 |
{ |
83 | 80 |
"scout.png" |
... | ... | |
92 | 89 |
scout_images[i].SetMaskColour(255, 255, 255); |
93 | 90 |
} |
94 | 91 |
|
92 |
/// @todo This should change. |
|
95 | 93 |
meter = scout_images[0].GetHeight(); |
96 | 94 |
|
97 |
path_bitmap = wxBitmap(GetSize().GetWidth(), GetSize().GetHeight());
|
|
98 |
path_dc.SelectObject(path_bitmap);
|
|
95 |
base_map_name = map_name;
|
|
96 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" + map_name + ".bmp";
|
|
99 | 97 |
clear(); |
100 | 98 |
|
101 | 99 |
clear_srv = nh.advertiseService("clear", |
... | ... | |
110 | 108 |
ROS_INFO("Starting scoutsim with node name %s", |
111 | 109 |
ros::this_node::getName().c_str()) ; |
112 | 110 |
|
111 |
wxMenu *menuFile = new wxMenu; |
|
112 |
menuFile->Append(ID_ABOUT, _("&About")); |
|
113 |
menuFile->AppendSeparator(); |
|
114 |
menuFile->Append(ID_QUIT, _("E&xit")); |
|
115 |
|
|
116 |
wxMenu *menuSim = new wxMenu; |
|
117 |
menuSim->Append(ID_CLEAR, _("&Clear")); |
|
118 |
|
|
119 |
wxMenu *menuView = new wxMenu; |
|
120 |
menuView->Append(ID_MAP, _("&Map")); |
|
121 |
menuView->Append(ID_LINES, _("&Lines")); |
|
122 |
|
|
123 |
wxMenuBar *menuBar = new wxMenuBar; |
|
124 |
menuBar->Append(menuFile, _("&File")); |
|
125 |
menuBar->Append(menuSim, _("&Sim")); |
|
126 |
menuBar->Append(menuView, _("&View")); |
|
127 |
|
|
128 |
SetMenuBar(menuBar); |
|
129 |
|
|
113 | 130 |
width_in_meters = GetSize().GetWidth() / meter; |
114 | 131 |
height_in_meters = GetSize().GetHeight() / meter; |
115 | 132 |
spawnScout("", width_in_meters / 2.0, height_in_meters / 2.0, 0); |
... | ... | |
188 | 205 |
return real_name; |
189 | 206 |
} |
190 | 207 |
|
191 |
void SimFrame::clear() |
|
208 |
void SimFrame::onQuit(wxCommandEvent& WXUNUSED(event)) |
|
209 |
{ |
|
210 |
Close(true); |
|
211 |
} |
|
212 |
|
|
213 |
void SimFrame::onAbout(wxCommandEvent& WXUNUSED(event)) |
|
214 |
{ |
|
215 |
wxMessageBox(_("Scoutsim is the simulator the Colony Project's scout robot.\n" |
|
216 |
"\nThe Colony Project is a part of the Carnegie Mellon\n" |
|
217 |
"Robotics Club. Our goal is to use cooperative low-cost\n" |
|
218 |
"robots to solve challenging problems."), |
|
219 |
_("About Scoutsim"), |
|
220 |
wxOK | wxICON_INFORMATION, this ); |
|
221 |
} |
|
222 |
|
|
223 |
void SimFrame::onClear(wxCommandEvent& WXUNUSED(event)) |
|
192 | 224 |
{ |
193 |
int r = DEFAULT_BG_R; |
|
194 |
int g = DEFAULT_BG_G; |
|
195 |
int b = DEFAULT_BG_B; |
|
225 |
clear(); |
|
226 |
} |
|
196 | 227 |
|
197 |
nh.param("background_r", r, r); |
|
198 |
nh.param("background_g", g, g); |
|
199 |
nh.param("background_b", b, b); |
|
228 |
void SimFrame::showMap(wxCommandEvent& WXUNUSED(event)) |
|
229 |
{ |
|
230 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" + |
|
231 |
base_map_name + ".bmp"; |
|
232 |
clear(); |
|
233 |
} |
|
200 | 234 |
|
201 |
path_dc.SetBackground(wxBrush(wxColour(r, g, b))); |
|
235 |
void SimFrame::showLines(wxCommandEvent& WXUNUSED(event)) |
|
236 |
{ |
|
237 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" + |
|
238 |
base_map_name + "_lines.bmp"; |
|
239 |
clear(); |
|
240 |
} |
|
241 |
|
|
242 |
void SimFrame::clear() |
|
243 |
{ |
|
244 |
path_dc.SetBackground(wxBrush(wxColour(100, 100, 100))); |
|
202 | 245 |
path_dc.Clear(); |
246 |
|
|
247 |
path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str())); |
|
248 |
path_dc.SelectObject(path_bitmap); |
|
249 |
SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight())); |
|
203 | 250 |
} |
204 | 251 |
|
205 | 252 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
48 | 48 |
#include <wx/wx.h> |
49 | 49 |
#include <wx/event.h> |
50 | 50 |
#include <wx/timer.h> |
51 |
#include <wx/string.h> |
|
51 | 52 |
|
52 | 53 |
#include <ros/ros.h> |
53 | 54 |
|
... | ... | |
60 | 61 |
#include "scoutsim_internal.h" |
61 | 62 |
|
62 | 63 |
#define SCOUTSIM_NUM_SCOUTS 1 |
64 |
#define ID_ABOUT 1 |
|
65 |
#define ID_QUIT 2 |
|
66 |
#define ID_CLEAR 3 |
|
67 |
#define ID_MAP 4 |
|
68 |
#define ID_LINES 5 |
|
63 | 69 |
|
64 | 70 |
namespace scoutsim |
65 | 71 |
{ |
66 | 72 |
class SimFrame : public wxFrame |
67 | 73 |
{ |
68 | 74 |
public: |
69 |
SimFrame(wxWindow* parent); |
|
75 |
SimFrame(wxWindow* parent, std::string map_name);
|
|
70 | 76 |
~SimFrame(); |
71 | 77 |
|
72 | 78 |
std::string spawnScout(const std::string& name, |
73 | 79 |
float x, float y, float angle); |
74 | 80 |
|
81 |
void onQuit(wxCommandEvent& event); |
|
82 |
void onAbout(wxCommandEvent& event); |
|
83 |
void onClear(wxCommandEvent& event); |
|
84 |
void showMap(wxCommandEvent& event); |
|
85 |
void showLines(wxCommandEvent& event); |
|
86 |
|
|
87 |
DECLARE_EVENT_TABLE() |
|
88 |
|
|
75 | 89 |
private: |
76 | 90 |
void onUpdate(wxTimerEvent& evt); |
77 | 91 |
void onPaint(wxPaintEvent& evt); |
... | ... | |
109 | 123 |
float meter; |
110 | 124 |
float width_in_meters; |
111 | 125 |
float height_in_meters; |
126 |
|
|
127 |
std::string base_map_name; |
|
128 |
std::string display_map_name; |
|
112 | 129 |
}; |
113 | 130 |
|
114 | 131 |
} |
Also available in: Unified diff