diff --git a/CMakeLists.txt b/CMakeLists.txt index 51cf1dbba6c7adb536aba2cc648214e172820e8e..a1737a712152f3252fd171a4e98b87b959ec72de 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/turtlesim/turtle.h b/include/turtlesim/turtle.h index 6cd013b18eca69cc61f2a10773192678c999f74b..ad371236850958bcf50e51c63d0726274fa8c75f 100644 --- a/include/turtlesim/turtle.h +++ b/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 { diff --git a/include/turtlesim/turtle_frame.h b/include/turtlesim/turtle_frame.h index 34b8f9f2910543096d3ecaf7d09add7bf76d1073..069f687f4bb1ec8961641b09aa6c876c4a777909 100644 --- a/include/turtlesim/turtle_frame.h +++ b/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_; diff --git a/msg/img.msg b/msg/img.msg deleted file mode 100644 index f31755ac0fa1b98b3c391e9ded636772bfeece78..0000000000000000000000000000000000000000 --- a/msg/img.msg +++ /dev/null @@ -1,4 +0,0 @@ -int64 x -int64 y -uint8[] img -# string imgPath \ No newline at end of file diff --git a/src/turtle.cpp b/src/turtle.cpp index 0fb2249e2768ec4d66aaae1c8651ec79dbfc4c91..bb128e813c9985bde2a1d6d817672650f44456ca 100644 --- a/src/turtle.cpp +++ b/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); } } diff --git a/src/turtle_frame.cpp b/src/turtle_frame.cpp index 64f726c0b5ef31c595a69b4dbc00cf8464e97729..55cff29401d151b7f9245f3df02f92e298fc9bd6 100644 --- a/src/turtle_frame.cpp +++ b/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; } } diff --git a/src/turtlesim_xl.cpp b/src/turtlesim_xl.cpp index 57ecea8f193decbd5cdccb0b1631616212c2db7a..1d5cac146ffd4c31297076fbf16105dc2c56a9cc 100644 --- a/src/turtlesim_xl.cpp +++ b/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(); } diff --git a/srv/SetLabel.srv b/srv/SetLabel.srv new file mode 100644 index 0000000000000000000000000000000000000000..ac4c27dc2a1886c5561a41daafc9803720496d35 --- /dev/null +++ b/srv/SetLabel.srv @@ -0,0 +1 @@ +string name \ No newline at end of file diff --git a/srv/img.srv b/srv/img.srv new file mode 100644 index 0000000000000000000000000000000000000000..171cd78878a30fd9adc100ee6147299662516dae --- /dev/null +++ b/srv/img.srv @@ -0,0 +1,3 @@ +int64 x +int64 y +uint8[] img \ No newline at end of file