00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "turtlesim/turtle_frame.h"
00031
00032 #include <QPointF>
00033
00034 #include <ros/package.h>
00035 #include <cstdlib>
00036 #include <ctime>
00037
00038 #define DEFAULT_BG_R 0x45
00039 #define DEFAULT_BG_G 0x56
00040 #define DEFAULT_BG_B 0xff
00041
00042 namespace turtlesim
00043 {
00044
00045 TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)
00046 : QFrame(parent, f)
00047 , path_image_(500, 500, QImage::Format_ARGB32)
00048 , path_painter_(&path_image_)
00049 , frame_count_(0)
00050 , id_counter_(0)
00051 {
00052 setFixedSize(500, 500);
00053 setWindowTitle("TurtleSim");
00054
00055 srand(time(NULL));
00056
00057 update_timer_ = new QTimer(this);
00058 update_timer_->setInterval(16);
00059 update_timer_->start();
00060
00061 connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate()));
00062
00063 nh_.setParam("background_r", DEFAULT_BG_R);
00064 nh_.setParam("background_g", DEFAULT_BG_G);
00065 nh_.setParam("background_b", DEFAULT_BG_B);
00066
00067 QVector<QString> turtles;
00068 turtles.append("box-turtle.png");
00069 turtles.append("robot-turtle.png");
00070 turtles.append("sea-turtle.png");
00071 turtles.append("diamondback.png");
00072 turtles.append("electric.png");
00073 turtles.append("fuerte.png");
00074 turtles.append("groovy.png");
00075 turtles.append("hydro.svg");
00076
00077 QString images_path = (ros::package::getPath("turtlesim") + "/images/").c_str();
00078 for (size_t i = 0; i < turtles.size(); ++i)
00079 {
00080 QImage img;
00081 img.load(images_path + turtles[i]);
00082 turtle_images_.append(img);
00083 }
00084
00085 meter_ = turtle_images_[0].height();
00086
00087 clear();
00088
00089 clear_srv_ = nh_.advertiseService("clear", &TurtleFrame::clearCallback, this);
00090 reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this);
00091 spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this);
00092 kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this);
00093
00094 ROS_INFO("Starting turtlesim with node name %s", ros::this_node::getName().c_str()) ;
00095
00096 width_in_meters_ = (width() - 1) / meter_;
00097 height_in_meters_ = (height() - 1) / meter_;
00098 spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
00099
00100
00101 if(FALSE)
00102 {
00103 for(size_t index = 0; index < turtles.size(); index++)
00104 {
00105 QString name = turtles[index];
00106 name = name.split(".").first();
00107 name.replace(QString("-"), QString(""));
00108 spawnTurtle(name.toStdString(), 1.0 + 1.5 * (index % 7), 1.0 + 1.5 * (index / 7), PI / 2.0, index);
00109 }
00110 }
00111 }
00112
00113 TurtleFrame::~TurtleFrame()
00114 {
00115 delete update_timer_;
00116 }
00117
00118 bool TurtleFrame::spawnCallback(turtlesim::Spawn::Request& req, turtlesim::Spawn::Response& res)
00119 {
00120 std::string name = spawnTurtle(req.name, req.x, req.y, req.theta);
00121 if (name.empty())
00122 {
00123 ROS_ERROR("A turtled named [%s] already exists", req.name.c_str());
00124 return false;
00125 }
00126
00127 res.name = name;
00128
00129 return true;
00130 }
00131
00132 bool TurtleFrame::killCallback(turtlesim::Kill::Request& req, turtlesim::Kill::Response&)
00133 {
00134 M_Turtle::iterator it = turtles_.find(req.name);
00135 if (it == turtles_.end())
00136 {
00137 ROS_ERROR("Tried to kill turtle [%s], which does not exist", req.name.c_str());
00138 return false;
00139 }
00140
00141 turtles_.erase(it);
00142 update();
00143
00144 return true;
00145 }
00146
00147 bool TurtleFrame::hasTurtle(const std::string& name)
00148 {
00149 return turtles_.find(name) != turtles_.end();
00150 }
00151
00152 std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle)
00153 {
00154 return spawnTurtle(name, x, y, angle, rand() % turtle_images_.size());
00155 }
00156
00157 std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y, float angle, size_t index)
00158 {
00159 std::string real_name = name;
00160 if (real_name.empty())
00161 {
00162 do
00163 {
00164 std::stringstream ss;
00165 ss << "turtle" << ++id_counter_;
00166 real_name = ss.str();
00167 } while (hasTurtle(real_name));
00168 }
00169 else
00170 {
00171 if (hasTurtle(real_name))
00172 {
00173 return "";
00174 }
00175 }
00176
00177 TurtlePtr t(new Turtle(ros::NodeHandle(real_name), turtle_images_[index], QPointF(x, y), angle));
00178 turtles_[real_name] = t;
00179 update();
00180
00181 ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);
00182
00183 return real_name;
00184 }
00185
00186 void TurtleFrame::clear()
00187 {
00188 int r = DEFAULT_BG_R;
00189 int g = DEFAULT_BG_G;
00190 int b = DEFAULT_BG_B;
00191
00192 nh_.param("background_r", r, r);
00193 nh_.param("background_g", g, g);
00194 nh_.param("background_b", b, b);
00195
00196 path_image_.fill(qRgb(r, g, b));
00197 update();
00198 }
00199
00200 void TurtleFrame::onUpdate()
00201 {
00202 ros::spinOnce();
00203
00204 updateTurtles();
00205
00206 if (!ros::ok())
00207 {
00208 close();
00209 }
00210 }
00211
00212 void TurtleFrame::paintEvent(QPaintEvent* event)
00213 {
00214 QPainter painter(this);
00215
00216 painter.drawImage(QPoint(0, 0), path_image_);
00217
00218 M_Turtle::iterator it = turtles_.begin();
00219 M_Turtle::iterator end = turtles_.end();
00220 for (; it != end; ++it)
00221 {
00222 it->second->paint(painter);
00223 }
00224 }
00225
00226 void TurtleFrame::updateTurtles()
00227 {
00228 if (last_turtle_update_.isZero())
00229 {
00230 last_turtle_update_ = ros::WallTime::now();
00231 return;
00232 }
00233
00234 bool modified = false;
00235 M_Turtle::iterator it = turtles_.begin();
00236 M_Turtle::iterator end = turtles_.end();
00237 for (; it != end; ++it)
00238 {
00239 modified |= it->second->update(0.001 * update_timer_->interval(), path_painter_, path_image_, width_in_meters_, height_in_meters_);
00240 }
00241 if (modified)
00242 {
00243 update();
00244 }
00245
00246 ++frame_count_;
00247 }
00248
00249
00250 bool TurtleFrame::clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00251 {
00252 ROS_INFO("Clearing turtlesim.");
00253 clear();
00254 return true;
00255 }
00256
00257 bool TurtleFrame::resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00258 {
00259 ROS_INFO("Resetting turtlesim.");
00260 turtles_.clear();
00261 id_counter_ = 0;
00262 spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
00263 clear();
00264 return true;
00265 }
00266
00267 }