turtle.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "turtlesim/turtle.h"
00031 
00032 #include <QColor>
00033 #include <QRgb>
00034 
00035 #define DEFAULT_PEN_R 0xb3
00036 #define DEFAULT_PEN_G 0xb8
00037 #define DEFAULT_PEN_B 0xff
00038 
00039 namespace turtlesim
00040 {
00041 
00042 Turtle::Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, const QPointF& pos, float orient)
00043 : nh_(nh)
00044 , turtle_image_(turtle_image)
00045 , pos_(pos)
00046 , orient_(orient)
00047 , lin_vel_(0.0)
00048 , ang_vel_(0.0)
00049 , pen_on_(true)
00050 , pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
00051 {
00052   pen_.setWidth(3);
00053 
00054   velocity_sub_ = nh_.subscribe("cmd_vel", 1, &Turtle::velocityCallback, this);
00055   pose_pub_ = nh_.advertise<Pose>("pose", 1);
00056   color_pub_ = nh_.advertise<Color>("color_sensor", 1);
00057   set_pen_srv_ = nh_.advertiseService("set_pen", &Turtle::setPenCallback, this);
00058   teleport_relative_srv_ = nh_.advertiseService("teleport_relative", &Turtle::teleportRelativeCallback, this);
00059   teleport_absolute_srv_ = nh_.advertiseService("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
00060 
00061   meter_ = turtle_image_.height();
00062   rotateImage();
00063 }
00064 
00065 
00066 void Turtle::velocityCallback(const geometry_msgs::Twist::ConstPtr& vel)
00067 {
00068   last_command_time_ = ros::WallTime::now();
00069   lin_vel_ = vel->linear.x;
00070   ang_vel_ = vel->angular.z;
00071 }
00072 
00073 bool Turtle::setPenCallback(turtlesim::SetPen::Request& req, turtlesim::SetPen::Response&)
00074 {
00075   pen_on_ = !req.off;
00076   if (req.off)
00077   {
00078     return true;
00079   }
00080 
00081   QPen pen(QColor(req.r, req.g, req.b));
00082   if (req.width != 0)
00083   {
00084     pen.setWidth(req.width);
00085   }
00086 
00087   pen_ = pen;
00088   return true;
00089 }
00090 
00091 bool Turtle::teleportRelativeCallback(turtlesim::TeleportRelative::Request& req, turtlesim::TeleportRelative::Response&)
00092 {
00093   teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true));
00094   return true;
00095 }
00096 
00097 bool Turtle::teleportAbsoluteCallback(turtlesim::TeleportAbsolute::Request& req, turtlesim::TeleportAbsolute::Response&)
00098 {
00099   teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, false));
00100   return true;
00101 }
00102 
00103 void Turtle::rotateImage()
00104 {
00105   QTransform transform;
00106   transform.rotate(-orient_ * 180.0 / PI + 90.0);
00107   turtle_rotated_image_ = turtle_image_.transformed(transform);
00108 }
00109 
00110 bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height)
00111 {
00112   bool modified = false;
00113   qreal old_orient = orient_;
00114 
00115   // first process any teleportation requests, in order
00116   V_TeleportRequest::iterator it = teleport_requests_.begin();
00117   V_TeleportRequest::iterator end = teleport_requests_.end();
00118   for (; it != end; ++it)
00119   {
00120     const TeleportRequest& req = *it;
00121 
00122     QPointF old_pos = pos_;
00123     if (req.relative)
00124     {
00125       orient_ += req.theta;
00126       pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear;
00127       pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear;
00128     }
00129     else
00130     {
00131       pos_.setX(req.pos.x());
00132       pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));
00133       orient_ = req.theta;
00134     }
00135 
00136     path_painter.setPen(pen_);
00137     path_painter.drawLine(pos_ * meter_, old_pos * meter_);
00138     modified = true;
00139   }
00140 
00141   teleport_requests_.clear();
00142 
00143   if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))
00144   {
00145     lin_vel_ = 0.0;
00146     ang_vel_ = 0.0;
00147   }
00148 
00149   QPointF old_pos = pos_;
00150 
00151   orient_ = std::fmod(orient_ + ang_vel_ * dt, 2*PI);
00152   pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;
00153   pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;
00154 
00155   // Clamp to screen size
00156   if (pos_.x() < 0 || pos_.x() > canvas_width ||
00157       pos_.y() < 0 || pos_.y() > canvas_height)
00158   {
00159     ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y());
00160   }
00161 
00162   pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), static_cast<double>(canvas_width)));
00163   pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), static_cast<double>(canvas_height)));
00164 
00165   // Publish pose of the turtle
00166   Pose p;
00167   p.x = pos_.x();
00168   p.y = canvas_height - pos_.y();
00169   p.theta = orient_;
00170   p.linear_velocity = lin_vel_;
00171   p.angular_velocity = ang_vel_;
00172   pose_pub_.publish(p);
00173 
00174   // Figure out (and publish) the color underneath the turtle
00175   {
00176     Color color;
00177     QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
00178     color.r = qRed(pixel);
00179     color.g = qGreen(pixel);
00180     color.b = qBlue(pixel);
00181     color_pub_.publish(color);
00182   }
00183 
00184   ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), pos_.x(), pos_.y(), orient_);
00185 
00186   if (orient_ != old_orient)
00187   {
00188     rotateImage();
00189     modified = true;
00190   }
00191   if (pos_ != old_pos)
00192   {
00193     if (pen_on_)
00194     {
00195       path_painter.setPen(pen_);
00196       path_painter.drawLine(pos_ * meter_, old_pos * meter_);
00197     }
00198     modified = true;
00199   }
00200 
00201   return modified;
00202 }
00203 
00204 void Turtle::paint(QPainter& painter)
00205 {
00206   QPointF p = pos_ * meter_;
00207   p.rx() -= 0.5 * turtle_rotated_image_.width();
00208   p.ry() -= 0.5 * turtle_rotated_image_.height();
00209   painter.drawImage(p, turtle_rotated_image_);
00210 }
00211 
00212 }


turtlesim
Author(s): Josh Faust
autogenerated on Thu Jun 6 2019 20:20:12