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


turtlesim
Author(s): Josh Faust
autogenerated on Fri Jun 7 2019 22:01:46