robobuggy / buggyvis / src / vis_frame.cpp @ 2079926a
History | View | Annotate | Download (8.18 KB)
1 | f75a88be | tahm | /*
|
---|---|---|---|
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 "buggyvis/vis_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 | 2079926a | Matt Sebek | setWindowTitle("BuggyVis!");
|
56 | f75a88be | tahm | |
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 | connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));
|
||
64 | |||
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 | |||
77 | map_image_ = new QImage(700, 500, QImage::Format_ARGB32); |
||
78 | |||
79 | // TODO: paint waypoints
|
||
80 | // TODO: load turtles automatically/change to buggy images
|
||
81 | |||
82 | /* Load turtle images */
|
||
83 | // Note: buggies must be added here to be chosen.
|
||
84 | // TODO: associate buggy with specific model.
|
||
85 | QVector<QString> turtles; |
||
86 | turtles.append("electric.png");
|
||
87 | |||
88 | QString images_path = (ros::package::getPath("buggysim") + "/images/").c_str(); |
||
89 | for (size_t i = 0; i < turtles.size(); ++i) { |
||
90 | QImage img; |
||
91 | img.load(images_path + turtles[i]); |
||
92 | turtle_images_.append(img); |
||
93 | } |
||
94 | |||
95 | // TODO: load meter/pixel ratio from parameter server
|
||
96 | meter_ = turtle_images_[0].height();
|
||
97 | |||
98 | clear(); |
||
99 | |||
100 | clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this); |
||
101 | reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this); |
||
102 | spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this); |
||
103 | kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this); |
||
104 | |||
105 | ROS_INFO("Starting buggysim with node name %s",
|
||
106 | ros::this_node::getName().c_str()) ; |
||
107 | |||
108 | width_in_meters_ = (width() - 1) / meter_;
|
||
109 | height_in_meters_ = (height() - 1) / meter_;
|
||
110 | |||
111 | // Spawn turtle in a specific location.
|
||
112 | spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); |
||
113 | |||
114 | ROS_INFO("Past constructor!\n");
|
||
115 | } |
||
116 | |||
117 | TurtleFrame::~TurtleFrame() |
||
118 | { |
||
119 | delete update_timer_;
|
||
120 | } |
||
121 | |||
122 | bool TurtleFrame::spawnCallback(buggysim::Spawn::Request& req,
|
||
123 | buggysim::Spawn::Response& res) |
||
124 | { |
||
125 | std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);
|
||
126 | if (name.empty()) {
|
||
127 | ROS_ERROR("A turtled named [%s] already exists", req.name.c_str());
|
||
128 | return false; |
||
129 | } |
||
130 | |||
131 | res.name = name; |
||
132 | |||
133 | return true; |
||
134 | } |
||
135 | |||
136 | bool TurtleFrame::killCallback(buggysim::Kill::Request& req,
|
||
137 | buggysim::Kill::Response&) |
||
138 | { |
||
139 | M_Turtle::iterator it = turtles_.find(req.name); |
||
140 | if (it == turtles_.end()) {
|
||
141 | ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());
|
||
142 | return false; |
||
143 | } |
||
144 | |||
145 | turtles_.erase(it); |
||
146 | update(); |
||
147 | |||
148 | return true; |
||
149 | } |
||
150 | |||
151 | bool TurtleFrame::hasTurtle(const std::string& name) |
||
152 | { |
||
153 | return turtles_.find(name) != turtles_.end();
|
||
154 | } |
||
155 | |||
156 | std::string TurtleFrame::spawnTurtle(const std::string& name, |
||
157 | float x,
|
||
158 | float y,
|
||
159 | float angle)
|
||
160 | { |
||
161 | std::string real_name = name;
|
||
162 | if (real_name.empty()) {
|
||
163 | do {
|
||
164 | std::stringstream ss; |
||
165 | // TODO: change how turtles are spawned
|
||
166 | ss << "buggy" << ++id_counter_;
|
||
167 | real_name = ss.str(); |
||
168 | } while (hasTurtle(real_name));
|
||
169 | } else {
|
||
170 | if (hasTurtle(real_name)) {
|
||
171 | return ""; |
||
172 | } |
||
173 | } |
||
174 | |||
175 | // Load the correct implementation of turtle to spawn into Screen
|
||
176 | TurtlePtr t(new Turtle(ros::NodeHandle(real_name),
|
||
177 | // Added parameter for wall image
|
||
178 | wall_image_, |
||
179 | turtle_images_[rand() % turtle_images_.size()], |
||
180 | QPointF(x, y), |
||
181 | angle)); |
||
182 | |||
183 | turtles_[real_name] = t; |
||
184 | update(); |
||
185 | |||
186 | ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]",
|
||
187 | real_name.c_str(), x, y, angle); |
||
188 | |||
189 | return real_name;
|
||
190 | } |
||
191 | |||
192 | void TurtleFrame::clear()
|
||
193 | { |
||
194 | int r = DEFAULT_BG_R;
|
||
195 | int g = DEFAULT_BG_G;
|
||
196 | int b = DEFAULT_BG_B;
|
||
197 | |||
198 | nh_.param("background_r", r, r);
|
||
199 | nh_.param("background_g", g, g);
|
||
200 | nh_.param("background_b", b, b);
|
||
201 | map_image_->load(map_path_); |
||
202 | update(); |
||
203 | } |
||
204 | |||
205 | void TurtleFrame::onUpdate()
|
||
206 | { |
||
207 | ros::spinOnce(); |
||
208 | |||
209 | updateTurtles(); |
||
210 | |||
211 | if (!ros::ok()) {
|
||
212 | close(); |
||
213 | } |
||
214 | } |
||
215 | |||
216 | void TurtleFrame::paintEvent(QPaintEvent* event)
|
||
217 | { |
||
218 | QPainter painter(this);
|
||
219 | |||
220 | painter.drawImage(QPoint(0, 0), (*map_image_)); |
||
221 | |||
222 | M_Turtle::iterator it = turtles_.begin(); |
||
223 | M_Turtle::iterator end = turtles_.end(); |
||
224 | for (; it != end; ++it) {
|
||
225 | it->second->paint(painter); |
||
226 | } |
||
227 | } |
||
228 | |||
229 | void TurtleFrame::updateTurtles()
|
||
230 | { |
||
231 | |||
232 | if (last_turtle_update_.isZero()) {
|
||
233 | last_turtle_update_ = ros::WallTime::now(); |
||
234 | return;
|
||
235 | } |
||
236 | bool modified = false; |
||
237 | |||
238 | QPainter this_painter(map_image_); |
||
239 | |||
240 | M_Turtle::iterator it = turtles_.begin(); |
||
241 | M_Turtle::iterator end = turtles_.end(); |
||
242 | for (; it != end; ++it) {
|
||
243 | 066966cf | tahm | modified |= it->second->update( |
244 | f75a88be | tahm | this_painter, |
245 | (*map_image_), |
||
246 | width_in_meters_, |
||
247 | height_in_meters_); |
||
248 | } |
||
249 | if (modified) {
|
||
250 | update(); |
||
251 | } |
||
252 | |||
253 | ++frame_count_; |
||
254 | } |
||
255 | |||
256 | |||
257 | bool TurtleFrame::clearCallback(std_srvs::Empty::Request&,
|
||
258 | std_srvs::Empty::Response&) |
||
259 | { |
||
260 | ROS_INFO("Clearing turtlesim.");
|
||
261 | clear(); |
||
262 | return true; |
||
263 | } |
||
264 | |||
265 | // Don't use this code; it will explode.
|
||
266 | // This must be updated to handle more complicated sim
|
||
267 | bool TurtleFrame::resetCallback(std_srvs::Empty::Request&,
|
||
268 | std_srvs::Empty::Response&) |
||
269 | { |
||
270 | ROS_INFO("Resetting turtlesim.");
|
||
271 | turtles_.clear(); |
||
272 | id_counter_ = 0;
|
||
273 | spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); |
||
274 | clear(); |
||
275 | int a = 1/0; |
||
276 | return true; |
||
277 | } |
||
278 | } |