robobuggy / buggysim / src / sim_frame.cpp @ f75a88be
History | View | Annotate | Download (8.68 KB)
1 |
/*
|
---|---|
2 |
* Copyright (c) 2009, Willow Garage, Inc.
|
3 |
* All rights reserved.
|
4 |
*
|
5 |
* Redistribution and use in source and binary forms, with or without
|
6 |
* modification, are permitted provided that the following conditions are met:
|
7 |
*
|
8 |
* * Redistributions of source code must retain the above copyright
|
9 |
* notice, this list of conditions and the following disclaimer.
|
10 |
* * Redistributions in binary form must reproduce the above copyright
|
11 |
* notice, this list of conditions and the following disclaimer in the
|
12 |
* documentation and/or other materials provided with the distribution.
|
13 |
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
14 |
* contributors may be used to endorse or promote products derived from
|
15 |
* this software without specific prior written permission.
|
16 |
*
|
17 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
18 |
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
19 |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
20 |
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
21 |
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
22 |
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
23 |
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
24 |
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
25 |
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
26 |
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
27 |
* POSSIBILITY OF SUCH DAMAGE.
|
28 |
*/
|
29 |
|
30 |
#include "buggysim/sim_frame.h" |
31 |
|
32 |
|
33 |
#include <QPointF> |
34 |
|
35 |
#include <ros/package.h> |
36 |
#include <cstdlib> |
37 |
#include <ctime> |
38 |
|
39 |
#define DEFAULT_BG_R 0x45 |
40 |
#define DEFAULT_BG_G 0x56 |
41 |
#define DEFAULT_BG_B 0xff |
42 |
|
43 |
namespace buggysim
|
44 |
{ |
45 |
|
46 |
TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f) |
47 |
: QFrame(parent, f) |
48 |
, frame_count_(0)
|
49 |
, id_counter_(0)
|
50 |
{ |
51 |
|
52 |
|
53 |
// Same as image size
|
54 |
setFixedSize(700, 500); |
55 |
setWindowTitle("BuggySim Frame!");
|
56 |
|
57 |
srand(time(NULL));
|
58 |
|
59 |
// QT window updater/timer
|
60 |
update_timer_ = new QTimer(this); |
61 |
update_timer_->setInterval(16);
|
62 |
update_timer_->start(); |
63 |
|
64 |
connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));
|
65 |
|
66 |
nh_.setParam("background_r", DEFAULT_BG_R);
|
67 |
nh_.setParam("background_g", DEFAULT_BG_G);
|
68 |
nh_.setParam("background_b", DEFAULT_BG_B);
|
69 |
|
70 |
|
71 |
/* Set up map */
|
72 |
// TODO: retrieve map names from param server
|
73 |
std::string toBeRemovedMapName = "race"; |
74 |
map_path_ = (ros::package::getPath("buggymaps") + "/maps/" + |
75 |
toBeRemovedMapName + ".bmp").c_str();
|
76 |
wall_path_ = (ros::package::getPath("buggymaps") + "/maps/" + |
77 |
toBeRemovedMapName + "_walls.bmp").c_str();
|
78 |
wall_image_.load(wall_path_); |
79 |
|
80 |
map_image_ = new QImage(700, 500, QImage::Format_ARGB32); |
81 |
|
82 |
// TODO: paint waypoints
|
83 |
// TODO: load turtles automatically/change to buggy images
|
84 |
|
85 |
/* Load turtle images */
|
86 |
// Note: buggies must be added here to be chosen.
|
87 |
// TODO: associate buggy with specific model.
|
88 |
QVector<QString> turtles; |
89 |
//turtles.append("robot-turtle.png");
|
90 |
turtles.append("electric.png");
|
91 |
//turtles.append("fuerte.png");
|
92 |
//turtles.append("groovy.png");
|
93 |
//turtles.append("hydro.svg");
|
94 |
|
95 |
QString images_path = (ros::package::getPath("buggysim") + "/images/").c_str(); |
96 |
for (size_t i = 0; i < turtles.size(); ++i) { |
97 |
QImage img; |
98 |
img.load(images_path + turtles[i]); |
99 |
turtle_images_.append(img); |
100 |
} |
101 |
|
102 |
// TODO: load meter/pixel ratio from parameter server
|
103 |
meter_ = turtle_images_[0].height();
|
104 |
|
105 |
|
106 |
clear(); |
107 |
|
108 |
clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this); |
109 |
reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this); |
110 |
spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this); |
111 |
kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this); |
112 |
|
113 |
ROS_INFO("Starting buggysim with node name %s",
|
114 |
ros::this_node::getName().c_str()) ; |
115 |
|
116 |
width_in_meters_ = (width() - 1) / meter_;
|
117 |
height_in_meters_ = (height() - 1) / meter_;
|
118 |
|
119 |
// Spawn turtle in a specific location.
|
120 |
spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); |
121 |
|
122 |
ROS_INFO("Past constructor!\n");
|
123 |
} |
124 |
|
125 |
TurtleFrame::~TurtleFrame() |
126 |
{ |
127 |
delete update_timer_;
|
128 |
} |
129 |
|
130 |
bool TurtleFrame::spawnCallback(buggysim::Spawn::Request& req,
|
131 |
buggysim::Spawn::Response& res) |
132 |
{ |
133 |
std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);
|
134 |
if (name.empty()) {
|
135 |
ROS_ERROR("A turtled named [%s] already exists", req.name.c_str());
|
136 |
return false; |
137 |
} |
138 |
|
139 |
res.name = name; |
140 |
|
141 |
return true; |
142 |
} |
143 |
|
144 |
bool TurtleFrame::killCallback(buggysim::Kill::Request& req,
|
145 |
buggysim::Kill::Response&) |
146 |
{ |
147 |
M_Turtle::iterator it = turtles_.find(req.name); |
148 |
if (it == turtles_.end()) {
|
149 |
ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());
|
150 |
return false; |
151 |
} |
152 |
|
153 |
turtles_.erase(it); |
154 |
update(); |
155 |
|
156 |
return true; |
157 |
} |
158 |
|
159 |
bool TurtleFrame::hasTurtle(const std::string& name) |
160 |
{ |
161 |
return turtles_.find(name) != turtles_.end();
|
162 |
} |
163 |
|
164 |
std::string TurtleFrame::spawnTurtle(const std::string& name, |
165 |
float x,
|
166 |
float y,
|
167 |
float angle)
|
168 |
{ |
169 |
std::string real_name = name;
|
170 |
if (real_name.empty()) {
|
171 |
do {
|
172 |
std::stringstream ss; |
173 |
// TODO: change how turtles are spawned
|
174 |
ss << "buggy" << ++id_counter_;
|
175 |
real_name = ss.str(); |
176 |
} while (hasTurtle(real_name));
|
177 |
} else {
|
178 |
if (hasTurtle(real_name)) {
|
179 |
return ""; |
180 |
} |
181 |
} |
182 |
|
183 |
// Load the correct implementation of turtle to spawn into Screen
|
184 |
TurtlePtr t(new Turtle(ros::NodeHandle(real_name),
|
185 |
// Added parameter for wall image
|
186 |
wall_image_, |
187 |
turtle_images_[rand() % turtle_images_.size()], |
188 |
QPointF(x, y), |
189 |
angle)); |
190 |
|
191 |
turtles_[real_name] = t; |
192 |
update(); |
193 |
|
194 |
ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]",
|
195 |
real_name.c_str(), x, y, angle); |
196 |
|
197 |
return real_name;
|
198 |
} |
199 |
|
200 |
void TurtleFrame::clear()
|
201 |
{ |
202 |
int r = DEFAULT_BG_R;
|
203 |
int g = DEFAULT_BG_G;
|
204 |
int b = DEFAULT_BG_B;
|
205 |
|
206 |
nh_.param("background_r", r, r);
|
207 |
nh_.param("background_g", g, g);
|
208 |
nh_.param("background_b", b, b);
|
209 |
// TODO: reload the map image
|
210 |
//map_image_.fill(qRgb(r, g, b));
|
211 |
map_image_->load(map_path_); |
212 |
update(); |
213 |
} |
214 |
|
215 |
void TurtleFrame::onUpdate()
|
216 |
{ |
217 |
ros::spinOnce(); |
218 |
|
219 |
updateTurtles(); |
220 |
|
221 |
if (!ros::ok()) {
|
222 |
close(); |
223 |
} |
224 |
} |
225 |
|
226 |
void TurtleFrame::paintEvent(QPaintEvent* event)
|
227 |
{ |
228 |
QPainter painter(this);
|
229 |
|
230 |
painter.drawImage(QPoint(0, 0), (*map_image_)); |
231 |
|
232 |
M_Turtle::iterator it = turtles_.begin(); |
233 |
M_Turtle::iterator end = turtles_.end(); |
234 |
for (; it != end; ++it) {
|
235 |
it->second->paint(painter); |
236 |
} |
237 |
} |
238 |
|
239 |
void TurtleFrame::updateTurtles()
|
240 |
{ |
241 |
|
242 |
if (last_turtle_update_.isZero()) {
|
243 |
last_turtle_update_ = ros::WallTime::now(); |
244 |
return;
|
245 |
} |
246 |
//ROS_INFO("Inside update\n");
|
247 |
bool modified = false; |
248 |
|
249 |
QPainter this_painter(map_image_); |
250 |
|
251 |
M_Turtle::iterator it = turtles_.begin(); |
252 |
M_Turtle::iterator end = turtles_.end(); |
253 |
for (; it != end; ++it) {
|
254 |
modified |= it->second->update(0.001 * update_timer_->interval(), |
255 |
this_painter, |
256 |
(*map_image_), |
257 |
width_in_meters_, |
258 |
height_in_meters_); |
259 |
} |
260 |
if (modified) {
|
261 |
update(); |
262 |
} |
263 |
|
264 |
++frame_count_; |
265 |
//ROS_INFO("Outside of update\n");
|
266 |
} |
267 |
|
268 |
|
269 |
bool TurtleFrame::clearCallback(std_srvs::Empty::Request&,
|
270 |
std_srvs::Empty::Response&) |
271 |
{ |
272 |
ROS_INFO("Clearing turtlesim.");
|
273 |
clear(); |
274 |
return true; |
275 |
} |
276 |
|
277 |
// Don't use this code; it will explode.
|
278 |
// This must be updated to handle more complicated sim
|
279 |
bool TurtleFrame::resetCallback(std_srvs::Empty::Request&,
|
280 |
std_srvs::Empty::Response&) |
281 |
{ |
282 |
ROS_INFO("Resetting turtlesim.");
|
283 |
turtles_.clear(); |
284 |
id_counter_ = 0;
|
285 |
spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); |
286 |
clear(); |
287 |
int a = 1/0; |
288 |
return true; |
289 |
} |
290 |
} |