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 "turtlesim/turtle.h"
  31 
  32 #include <QColor>
  33 #include <QRgb>
  34 
  35 #define DEFAULT_PEN_R 0xb3
  36 #define DEFAULT_PEN_G 0xb8
  37 #define DEFAULT_PEN_B 0xff
  38 
  39 using namespace std;
  40 
  41 namespace turtlesim
  42 {
  43 
  44 Turtle::Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, const QPointF& pos, float orient)
  45 : nh_(nh)
  46 , turtle_image_(turtle_image)
  47 , pos_(pos)
  48 , orient_(orient)
  49 , lin_vel_(0.0)
  50 , ang_vel_(0.0)
  51 , pen_on_(true)
  52 , pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
  53 {
  54   pen_.setWidth(3);
  55 
  56   velocity_sub_ = nh_.subscribe("command_velocity", 1, &Turtle::velocityCallback, this);
  57   pose_pub_ = nh_.advertise<Pose>("pose", 1);
  58   color_pub_ = nh_.advertise<Color>("color_sensor", 1);
  59   set_pen_srv_ = nh_.advertiseService("set_pen", &Turtle::setPenCallback, this);
  60   teleport_relative_srv_ = nh_.advertiseService("teleport_relative", &Turtle::teleportRelativeCallback, this);
  61   teleport_absolute_srv_ = nh_.advertiseService("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
  62 
  63   meter_ = turtle_image_.height();
  64   rotateImage();
  65 }
  66 
  67 
  68 void Turtle::velocityCallback(const VelocityConstPtr& vel)
  69 {
  70   last_command_time_ = ros::WallTime::now();
  71   lin_vel_ = vel->linear;
  72   ang_vel_ = vel->angular;
  73 }
  74 
  75 bool Turtle::setPenCallback(turtlesim::SetPen::Request& req, turtlesim::SetPen::Response&)
  76 {
  77   pen_on_ = !req.off;
  78   if (req.off)
  79   {
  80     return true;
  81   }
  82 
  83   QPen pen(QColor(req.r, req.g, req.b));
  84   if (req.width != 0)
  85   {
  86     pen.setWidth(req.width);
  87   }
  88 
  89   pen_ = pen;
  90   return true;
  91 }
  92 
  93 bool Turtle::teleportRelativeCallback(turtlesim::TeleportRelative::Request& req, turtlesim::TeleportRelative::Response&)
  94 {
  95   teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true));
  96   return true;
  97 }
  98 
  99 bool Turtle::teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request& req, turtlesim::TeleportAbsolute::Response&)
 100 {
 101   teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, false));
 102   return true;
 103 }
 104 
 105 void Turtle::rotateImage()
 106 {
 107   QTransform transform;
 108   transform.rotate(-orient_ * 180.0 / PI + 90.0);
 109   turtle_rotated_image_ = turtle_image_.transformed(transform);
 110 }
 111 using namespace std;
 112 bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height)
 113 {
 114   bool modified = false;
 115   qreal old_orient = orient_;
 116 
 117   // first process any teleportation requests, in order
 118   V_TeleportRequest::iterator it = teleport_requests_.begin();
 119   V_TeleportRequest::iterator end = teleport_requests_.end();
 120   for (; it != end; ++it)
 121   {
 122     const TeleportRequest& req = *it;
 123 
 124     QPointF old_pos = pos_;
 125     if (req.relative)
 126     {
 127       orient_ += req.theta;
 128       pos_.rx() += sin(orient_ + PI/2.0) * req.linear;
 129       pos_.ry() += cos(orient_ + PI/2.0) * req.linear;
 130     }
 131     else
 132     {
 133       pos_.setX(req.pos.x());
 134       pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));
 135       orient_ = req.theta;
 136     }
 137 
 138     path_painter.setPen(pen_);
 139     path_painter.drawLine(pos_ * meter_, old_pos * meter_);
 140     modified = true;
 141   }
 142 
 143   teleport_requests_.clear();
 144 
 145   if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))
 146   {
 147     lin_vel_ = 0.0;
 148     ang_vel_ = 0.0;
 149   }
 150 
 151   QPointF old_pos = pos_;
 152 
 153   orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);
 154   pos_.rx() += sin(orient_ + PI/2.0) * lin_vel_ * dt;
 155   pos_.ry() += cos(orient_ + PI/2.0) * lin_vel_ * dt;
 156 
 157   // Clamp to screen size
 158   if (pos_.x() < 0 || pos_.x() > canvas_width ||
 159       pos_.y() < 0 || pos_.y() > canvas_height)
 160   {
 161     ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y());
 162   }
 163 
 164   pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), static_cast<double>(canvas_width)));
 165   pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), static_cast<double>(canvas_height)));
 166 
 167   // Publish pose of the turtle
 168   Pose p;
 169   p.x = pos_.x();
 170   p.y = canvas_height - pos_.y();
 171   p.theta = orient_;
 172   p.linear_velocity = lin_vel_;
 173   p.angular_velocity = ang_vel_;
 174   pose_pub_.publish(p);
 175 
 176   // Figure out (and publish) the color underneath the turtle
 177   {
 178     Color color;
 179     QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
 180     color.r = qRed(pixel);
 181     color.g = qGreen(pixel);
 182     color.b = qBlue(pixel);
 183     color_pub_.publish(color);
 184   }
 185 
 186   ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), pos_.x(), pos_.y(), orient_);
 187 
 188   if (orient_ != old_orient)
 189   {
 190     rotateImage();
 191     modified = true;
 192   }
 193   if (pos_ != old_pos)
 194   {
 195     if (pen_on_)
 196     {
 197       path_painter.setPen(pen_);
 198       path_painter.drawLine(pos_ * meter_, old_pos * meter_);
 199     }
 200     modified = true;
 201   }
 202 
 203   return modified;
 204 }
 205 
 206 void Turtle::paint(QPainter& painter)
 207 {
 208   QPointF p = pos_ * meter_;
 209   p.rx() -= 0.5 * turtle_rotated_image_.width();
 210   p.ry() -= 0.5 * turtle_rotated_image_.height();
 211   painter.drawImage(p, turtle_rotated_image_);
 212 }
 213 
 214 }