DrawImage is now a service, added a label for turtles

CMakeLists.txt

@@ -20,9 +20,11 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 link_directories(${catkin_LIBRARY_DIRS})
 
 add_message_files(DIRECTORY msg FILES
-                  Color.msg Pose.msg img.msg)
+                  Color.msg Pose.msg)
 
 add_service_files(DIRECTORY srv FILES
+                  img.srv
+                  SetLabel.srv
                   Kill.srv
                   SetPen.srv
                   Spawn.srv

include/turtlesim/turtle.h

@@ -38,6 +38,7 @@
 # include <turtlesim/Pose.h>
 # include <geometry_msgs/Twist.h>
 # include <turtlesim/SetPen.h>
+# include <turtlesim_xl/SetLabel.h>
 # include <turtlesim/TeleportRelative.h>
 # include <turtlesim/TeleportAbsolute.h>
 # include <turtlesim/Color.h>
@@ -47,6 +48,7 @@
 #include <QPainter>
 #include <QPen>
 #include <QPointF>
+#include <QString> 
 
 #define PI 3.14159265
 
@@ -65,6 +67,7 @@ private:
   bool setPenCallback(turtlesim::SetPen::Request&, turtlesim::SetPen::Response&);
   bool teleportRelativeCallback(turtlesim::TeleportRelative::Request&, turtlesim::TeleportRelative::Response&);
   bool teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request&, turtlesim::TeleportAbsolute::Response&);
+  bool setLabelCallback(turtlesim_xl::SetLabel::Request& req, turtlesim_xl::SetLabel::Response&);
 
   void rotateImage();
 
@@ -87,12 +90,14 @@ private:
   ros::Publisher color_pub_;
   ros::Publisher inside_pub_;
   ros::ServiceServer set_pen_srv_;
+  ros::ServiceServer set_label_srv_;
   ros::ServiceServer teleport_relative_srv_;
   ros::ServiceServer teleport_absolute_srv_;
 
   ros::WallTime last_command_time_;
 
   float meter_;
+  QString label = "0";
 
   struct TeleportRequest
   {

include/turtlesim/turtle_frame.h

@@ -33,7 +33,6 @@
 #include <QPaintEvent>
 #include <QTimer>
 #include <QVector>
-#include <turtlesim_xl/img.h>
 
 // This prevents a MOC error with versions of boost >= 1.48
 #ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
@@ -42,6 +41,7 @@
 # include <std_srvs/Empty.h>
 # include <turtlesim/Spawn.h>
 # include <turtlesim/Kill.h>
+# include <turtlesim_xl/img.h>
 # include <map>
 
 # include "turtle.h"
@@ -60,7 +60,7 @@ public:
   std::string spawnTurtle(const std::string& name, float x, float y, float angle);
   std::string spawnTurtle(const std::string& name, float x, float y, float angle, size_t index);
 
-  void drawImage(turtlesim_xl::img img);
+  //void drawImage(turtlesim_xl::img img);
 
 protected:
   void paintEvent(QPaintEvent* event);
@@ -77,6 +77,7 @@ private:
   bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
   bool spawnCallback(turtlesim::Spawn::Request&, turtlesim::Spawn::Response&);
   bool killCallback(turtlesim::Kill::Request&, turtlesim::Kill::Response&);
+  bool drawImageCallback(turtlesim_xl::img::Request&, turtlesim_xl::img::Response&);
 
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
@@ -92,6 +93,7 @@ private:
   ros::ServiceServer reset_srv_;
   ros::ServiceServer spawn_srv_;
   ros::ServiceServer kill_srv_;
+  ros::ServiceServer img_srv_;
 
   typedef std::map<std::string, TurtlePtr> M_Turtle;
   M_Turtle turtles_;

src/turtle.cpp

@@ -56,6 +56,7 @@ Turtle::Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, const QPoi
   pose_pub_ = nh_.advertise<Pose>("pose", 1);
   color_pub_ = nh_.advertise<Color>("color_sensor", 1);
   set_pen_srv_ = nh_.advertiseService("set_pen", &Turtle::setPenCallback, this);
+  set_label_srv_ = nh_.advertiseService("set_label", &Turtle::setLabelCallback, this);
   teleport_relative_srv_ = nh_.advertiseService("teleport_relative", &Turtle::teleportRelativeCallback, this);
   teleport_absolute_srv_ = nh_.advertiseService("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
 
@@ -90,6 +91,10 @@ bool Turtle::setPenCallback(turtlesim::SetPen::Request& req, turtlesim::SetPen::
   return true;
 }
 
+bool Turtle::setLabelCallback(turtlesim_xl::SetLabel::Request& req, turtlesim_xl::SetLabel::Response&){
+  Turtle::label = QString(req.name.c_str());
+  return true;
+}
 bool Turtle::teleportRelativeCallback(turtlesim::TeleportRelative::Request& req, turtlesim::TeleportRelative::Response&)
 {
   teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true));
@@ -191,21 +196,6 @@ bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image,
     color_pub_.publish(color);
   }
 
-  ////return true if turtle is on a white pixel
-  //{
-  //  Color color;
-  //  QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
-  //  color.r = qRed(pixel);
-  //  color.g = qGreen(pixel);
-  //  color.b = qBlue(pixel);
-  //  ROS_INFO("red----------%d",color.r);
-  //  std_msgs::String a;
-  //  a.data="1";
-  //  std_msgs::String b;
-  //  b.data="1";
-  //  inside_pub_.publish((color.r==0xff&&color.g==0xff&&color.b==0xff)?a:b);
-  //}
-
 
   ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), pos_.x(), pos_.y(), orient_);
 
@@ -230,9 +220,14 @@ bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image,
 void Turtle::paint(QPainter& painter)
 {
   QPointF p = pos_ * meter_;
+  QPointF pl = pos_ * meter_;
+  pl.rx() -= 0.3 * turtle_rotated_image_.width();
+  //pl.ry() -= 0.5 * turtle_rotated_image_.height();
   p.rx() -= 0.5 * turtle_rotated_image_.width();
   p.ry() -= 0.5 * turtle_rotated_image_.height();
   painter.drawImage(p, turtle_rotated_image_);
+  painter.setPen(Qt::red);
+  painter.drawText(pl,Turtle::label);
 }
 
 }

src/turtle_frame.cpp

@@ -28,7 +28,6 @@
  */
 
 #include "turtlesim/turtle_frame.h"
-#include <turtlesim_xl/img.h>
 
 #include <QPointF>
 
@@ -111,6 +110,7 @@ TurtleFrame::TurtleFrame(QWidget* parent, Qt::WindowFlags f)
   reset_srv_ = nh_.advertiseService("reset", &TurtleFrame::resetCallback, this);
   spawn_srv_ = nh_.advertiseService("spawn", &TurtleFrame::spawnCallback, this);
   kill_srv_ = nh_.advertiseService("kill", &TurtleFrame::killCallback, this);
+  img_srv_ = nh_.advertiseService("img", &TurtleFrame::drawImageCallback, this);
 
   ROS_INFO("Starting turtlesim with node name %s", ros::this_node::getName().c_str()) ;
 
@@ -199,7 +199,7 @@ std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y,
   turtles_[real_name] = t;
   update();
 
-  ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);
+  //ROS_INFO("Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", real_name.c_str(), x, y, angle);
 
   return real_name;
 }
@@ -285,14 +285,15 @@ bool TurtleFrame::resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Resp
   return true;
 }
 
-void TurtleFrame::drawImage(turtlesim_xl::img imgData) {
+bool TurtleFrame::drawImageCallback(turtlesim_xl::img::Request& req, turtlesim_xl::img::Response&){
   QPointF point;
-  point.setX(imgData.x);
-  point.setY(FRAME_HEIGHT -imgData.y);
-	const u_char *c = reinterpret_cast<const u_char *>(&imgData.img[0]);
+  point.setX(req.x);
+  point.setY(FRAME_HEIGHT -req.y);
+	const u_char *c = reinterpret_cast<const u_char *>(&req.img[0]);
   QImage img;
-  img.loadFromData(c,imgData.img.size());
+  img.loadFromData(c,req.img.size());
   path_painter_.drawImage(point, img);
+  return true;
 }
 
 }

src/turtlesim_xl.cpp

@@ -32,12 +32,9 @@
 #include <ros/ros.h>
 
 #include "turtlesim/turtle_frame.h"
-#include <turtlesim_xl/img.h>
 
 class TurtleApp : public QApplication
 {
-  ros::Subscriber shapeSub;
-  turtlesim::TurtleFrame* frame;
 public:
   ros::NodeHandlePtr nh_;
 
@@ -45,38 +42,20 @@ public:
     : QApplication(argc, argv)
   {
     ros::init(argc, argv, "turtlesim", ros::init_options::NoSigintHandler);
-    ros::NodeHandle n;
     nh_.reset(new ros::NodeHandle);
-    // turtlesim::TurtleFrame fr;
-    // frame = &fr;
-    frame = new turtlesim::TurtleFrame();
-
-    shapeSub = n.subscribe("/shapeData", 10, &TurtleApp::updateShape, this);
   }
 
-  int exec(std::string shape = "rectangle")
+  int exec()
   {
-    frame->show();
+    turtlesim::TurtleFrame frame;
+    frame.show();
 
     return QApplication::exec();
   }
-
-private:
-  void updateShape(turtlesim_xl::img data) {
-      ROS_INFO("NEW SHAPE DATA SAVED");
-      frame->drawImage(data);
-  }
 };
 
 int main(int argc, char** argv)
 {
-  /*
-    argv with launch file:
-    0: path to node
-    1: additional argument (the result of shape=... in this case)
-    2: name of the node
-    3: path to log
-  */
   TurtleApp app(argc, argv);
   return app.exec();
 }

srv/SetLabel.srv

+string name
\ No newline at end of file

msg/img.msg → srv/img.srv

 int64 x
 int64 y
 uint8[] img
-# string imgPath
\ No newline at end of file