Revision 08be08ec
Ran astyle, though astyle options still need to be refined
buggysim/src/buggy.cpp | ||
---|---|---|
39 | 39 |
namespace buggysim |
40 | 40 |
{ |
41 | 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_(2.0) |
|
48 |
//, ang_vel_(0.0) |
|
49 |
, turnAngle_(0.0) |
|
50 |
, pen_on_(true) |
|
51 |
, pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) |
|
52 |
{ |
|
53 |
pen_.setWidth(3); |
|
54 |
|
|
55 |
angle_sub_ = nh_.subscribe |
|
56 |
("command_turnAngle", 1, &Turtle::turnAngleCallback, this); |
|
57 |
pose_pub_ = nh_.advertise<buggymsgs::Pose>("pose", 1); |
|
58 |
|
|
59 |
//color_pub_ = nh_.advertise<Color>("color_sensor", 1); |
|
60 |
set_pen_srv_ = nh_.advertiseService |
|
61 |
("set_pen", &Turtle::setPenCallback, this); |
|
62 |
teleport_relative_srv_ = nh_.advertiseService |
|
63 |
("teleport_relative", &Turtle::teleportRelativeCallback, this); |
|
64 |
teleport_absolute_srv_ = nh_.advertiseService |
|
65 |
("teleport_absolute", &Turtle::teleportAbsoluteCallback, this); |
|
66 |
|
|
67 |
meter_ = turtle_image_.height(); |
|
68 |
rotateImage(); |
|
69 |
} |
|
70 |
|
|
71 |
|
|
72 |
void Turtle::turnAngleCallback(const buggymsgs::DirectionConstPtr& dir) |
|
73 |
{ |
|
74 |
last_command_time_ = ros::WallTime::now(); |
|
75 |
turnAngle_ = dir->turnAngle; |
|
76 |
//lin_vel_ = vel->linear; |
|
77 |
//ang_vel_ = vel->angular; |
|
78 |
} |
|
79 |
|
|
80 |
bool Turtle::setPenCallback(buggysim::SetPen::Request& req, |
|
81 |
buggysim::SetPen::Response&) |
|
82 |
{ |
|
83 |
pen_on_ = !req.off; |
|
84 |
if (req.off) |
|
85 |
{ |
|
86 |
return true; |
|
87 |
} |
|
88 |
|
|
89 |
QPen pen(QColor(req.r, req.g, req.b)); |
|
90 |
if (req.width != 0) |
|
91 |
{ |
|
92 |
pen.setWidth(req.width); |
|
93 |
} |
|
94 |
|
|
95 |
pen_ = pen; |
|
96 |
return true; |
|
97 |
} |
|
98 |
|
|
99 |
bool Turtle::teleportRelativeCallback(buggysim::TeleportRelative::Request& req, |
|
100 |
buggysim::TeleportRelative::Response&) |
|
101 |
{ |
|
102 |
teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, true)); |
|
103 |
return true; |
|
104 |
} |
|
105 |
|
|
106 |
bool Turtle::teleportAbsoluteCallback(buggysim::TeleportAbsolute::Request& req, |
|
107 |
buggysim::TeleportAbsolute::Response&) |
|
108 |
{ |
|
109 |
teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, false)); |
|
110 |
return true; |
|
111 |
} |
|
112 |
|
|
113 |
void Turtle::rotateImage() |
|
114 |
{ |
|
115 |
QTransform transform; |
|
116 |
transform.rotate(-orient_ * 180.0 / PI + 90.0); |
|
117 |
turtle_rotated_image_ = turtle_image_.transformed(transform); |
|
118 |
} |
|
119 |
|
|
120 |
/** |
|
121 |
* TODO: convert this to take just an angle. And include drag. |
|
122 |
* and include acceleration/joule dump |
|
123 |
*/ |
|
124 |
bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, qreal canvas_width, qreal canvas_height) |
|
125 |
{ |
|
126 |
bool modified = false; |
|
127 |
qreal old_orient = orient_; |
|
128 |
|
|
129 |
// first process any teleportation requests, in order |
|
130 |
V_TeleportRequest::iterator it = teleport_requests_.begin(); |
|
131 |
V_TeleportRequest::iterator end = teleport_requests_.end(); |
|
132 |
for (; it != end; ++it) |
|
133 |
{ |
|
134 |
const TeleportRequest& req = *it; |
|
135 |
|
|
136 |
QPointF old_pos = pos_; |
|
137 |
if (req.relative) |
|
138 |
{ |
|
139 |
orient_ += req.theta; |
|
140 |
pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear; |
|
141 |
pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear; |
|
142 |
} |
|
143 |
else |
|
144 |
{ |
|
145 |
pos_.setX(req.pos.x()); |
|
146 |
pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y()))); |
|
147 |
orient_ = req.theta; |
|
148 |
} |
|
149 |
|
|
150 |
path_painter.setPen(pen_); |
|
151 |
path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
|
152 |
modified = true; |
|
153 |
} |
|
154 |
|
|
155 |
teleport_requests_.clear(); |
|
156 |
|
|
157 |
// Stop moving after 1 second since-last command. |
|
158 |
//if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0)) |
|
159 |
//{ |
|
160 |
// lin_vel_ = 0.0; |
|
161 |
// ang_vel_ = 0.0; |
|
162 |
//} |
|
163 |
|
|
164 |
// Update my position |
|
165 |
QPointF old_pos = pos_; |
|
166 |
|
|
167 |
/* Convert coordinate systens |
|
168 |
orient_ = std::fmod(orient_ + ang_vel_ * dt, 2*PI); |
|
169 |
pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt; |
|
170 |
pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt; |
|
171 |
*/ |
|
172 |
// TODO: what direction does orientation start at 0? |
|
173 |
// TODO: orient might have to become negative |
|
174 |
orient_ = std::fmod(orient_ - turnAngle_ * dt, 2*PI); |
|
175 |
// TODO: why add pi/2? is this just because they got the |
|
176 |
// sin/cos mixed up? |
|
177 |
//pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt; |
|
178 |
//pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt; |
|
179 |
|
|
180 |
pos_.rx() += std::cos(orient_) * lin_vel_ * dt; |
|
181 |
pos_.ry() += std::sin(orient_) * lin_vel_ * dt; |
|
182 |
|
|
183 |
|
|
184 |
// Find linear velocity |
|
185 |
//qreal lin_vel; |
|
186 |
// Find angular velocity |
|
187 |
//qreal ang_vel; |
|
188 |
|
|
189 |
// Clamp to screen size |
|
190 |
// TODO: change to use another image, to detect walls. |
|
191 |
if (pos_.x() < 0 || pos_.x() > canvas_width || |
|
192 |
pos_.y() < 0 || pos_.y() > canvas_height) |
|
193 |
{ |
|
194 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), pos_.y()); |
|
195 |
} |
|
196 |
|
|
197 |
pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), |
|
198 |
static_cast<double>(canvas_width))); |
|
199 |
pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), |
|
200 |
static_cast<double>(canvas_height))); |
|
201 |
|
|
202 |
// Publish pose of the turtle |
|
203 |
buggymsgs::Pose p; |
|
204 |
p.x = pos_.x(); |
|
205 |
p.y = canvas_height - pos_.y(); |
|
206 |
p.angle = orient_; |
|
207 |
p.linear_velocity = lin_vel_; |
|
208 |
//p.angular_velocity = ang_vel; |
|
209 |
pose_pub_.publish(p); |
|
210 |
|
|
211 |
// Figure out (and publish) the color underneath the turtle |
|
212 |
/*{ |
|
213 |
Color color; |
|
214 |
QRgb pixel = path_image.pixel((pos_ * meter_).toPoint()); |
|
215 |
color.r = qRed(pixel); |
|
216 |
color.g = qGreen(pixel); |
|
217 |
color.b = qBlue(pixel); |
|
218 |
color_pub_.publish(color); |
|
219 |
}*/ |
|
220 |
|
|
221 |
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), pos_.x(), pos_.y(), orient_); |
|
222 |
|
|
223 |
if (orient_ != old_orient) |
|
224 |
{ |
|
225 |
rotateImage(); |
|
226 |
modified = true; |
|
227 |
} |
|
228 |
if (pos_ != old_pos) |
|
229 |
{ |
|
230 |
if (pen_on_) |
|
231 |
{ |
|
232 |
path_painter.setPen(pen_); |
|
233 |
path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
|
234 |
} |
|
235 |
modified = true; |
|
236 |
} |
|
237 |
|
|
238 |
return modified; |
|
239 |
} |
|
240 |
|
|
241 |
void Turtle::paint(QPainter& painter) |
|
242 |
{ |
|
243 |
QPointF p = pos_ * meter_; |
|
244 |
p.rx() -= 0.5 * turtle_rotated_image_.width(); |
|
245 |
p.ry() -= 0.5 * turtle_rotated_image_.height(); |
|
246 |
painter.drawImage(p, turtle_rotated_image_); |
|
247 |
} |
|
42 |
Turtle::Turtle(const ros::NodeHandle& nh, const QImage& turtle_image, |
|
43 |
const QPointF& pos, float orient) |
|
44 |
: nh_(nh) |
|
45 |
, turtle_image_(turtle_image) |
|
46 |
, pos_(pos) |
|
47 |
, orient_(orient) |
|
48 |
, lin_vel_(2.0) |
|
49 |
//, ang_vel_(0.0) |
|
50 |
, turnAngle_(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 |
angle_sub_ = nh_.subscribe |
|
57 |
("command_turnAngle", 1, &Turtle::turnAngleCallback, this); |
|
58 |
pose_pub_ = nh_.advertise<buggymsgs::Pose>("pose", 1); |
|
59 |
|
|
60 |
//color_pub_ = nh_.advertise<Color>("color_sensor", 1); |
|
61 |
set_pen_srv_ = nh_.advertiseService |
|
62 |
("set_pen", &Turtle::setPenCallback, this); |
|
63 |
teleport_relative_srv_ = nh_.advertiseService |
|
64 |
("teleport_relative", &Turtle::teleportRelativeCallback, this); |
|
65 |
teleport_absolute_srv_ = nh_.advertiseService |
|
66 |
("teleport_absolute", &Turtle::teleportAbsoluteCallback, this); |
|
67 |
|
|
68 |
meter_ = turtle_image_.height(); |
|
69 |
rotateImage(); |
|
70 |
} |
|
71 |
|
|
72 |
|
|
73 |
void Turtle::turnAngleCallback(const buggymsgs::DirectionConstPtr& dir) |
|
74 |
{ |
|
75 |
last_command_time_ = ros::WallTime::now(); |
|
76 |
turnAngle_ = dir->turnAngle; |
|
77 |
//lin_vel_ = vel->linear; |
|
78 |
//ang_vel_ = vel->angular; |
|
79 |
} |
|
80 |
|
|
81 |
bool Turtle::setPenCallback(buggysim::SetPen::Request& req, |
|
82 |
buggysim::SetPen::Response&) |
|
83 |
{ |
|
84 |
pen_on_ = !req.off; |
|
85 |
if (req.off) { |
|
86 |
return true; |
|
87 |
} |
|
88 |
|
|
89 |
QPen pen(QColor(req.r, req.g, req.b)); |
|
90 |
if (req.width != 0) { |
|
91 |
pen.setWidth(req.width); |
|
92 |
} |
|
93 |
|
|
94 |
pen_ = pen; |
|
95 |
return true; |
|
96 |
} |
|
97 |
|
|
98 |
bool Turtle::teleportRelativeCallback(buggysim::TeleportRelative::Request& req, |
|
99 |
buggysim::TeleportRelative::Response&) |
|
100 |
{ |
|
101 |
teleport_requests_.push_back(TeleportRequest(0, 0, req.angular, req.linear, |
|
102 |
true)); |
|
103 |
return true; |
|
104 |
} |
|
105 |
|
|
106 |
bool Turtle::teleportAbsoluteCallback(buggysim::TeleportAbsolute::Request& req, |
|
107 |
buggysim::TeleportAbsolute::Response&) |
|
108 |
{ |
|
109 |
teleport_requests_.push_back(TeleportRequest(req.x, req.y, req.theta, 0, |
|
110 |
false)); |
|
111 |
return true; |
|
112 |
} |
|
113 |
|
|
114 |
void Turtle::rotateImage() |
|
115 |
{ |
|
116 |
QTransform transform; |
|
117 |
transform.rotate(-orient_ * 180.0 / PI + 90.0); |
|
118 |
turtle_rotated_image_ = turtle_image_.transformed(transform); |
|
119 |
} |
|
120 |
|
|
121 |
/** |
|
122 |
* TODO: convert this to take just an angle. And include drag. |
|
123 |
* and include acceleration/joule dump |
|
124 |
*/ |
|
125 |
bool Turtle::update(double dt, QPainter& path_painter, const QImage& path_image, |
|
126 |
qreal canvas_width, qreal canvas_height) |
|
127 |
{ |
|
128 |
bool modified = false; |
|
129 |
qreal old_orient = orient_; |
|
130 |
|
|
131 |
// first process any teleportation requests, in order |
|
132 |
V_TeleportRequest::iterator it = teleport_requests_.begin(); |
|
133 |
V_TeleportRequest::iterator end = teleport_requests_.end(); |
|
134 |
for (; it != end; ++it) { |
|
135 |
const TeleportRequest& req = *it; |
|
136 |
|
|
137 |
QPointF old_pos = pos_; |
|
138 |
if (req.relative) { |
|
139 |
orient_ += req.theta; |
|
140 |
pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear; |
|
141 |
pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear; |
|
142 |
} else { |
|
143 |
pos_.setX(req.pos.x()); |
|
144 |
pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y()))); |
|
145 |
orient_ = req.theta; |
|
146 |
} |
|
147 |
|
|
148 |
path_painter.setPen(pen_); |
|
149 |
path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
|
150 |
modified = true; |
|
151 |
} |
|
152 |
|
|
153 |
teleport_requests_.clear(); |
|
154 |
|
|
155 |
// Stop moving after 1 second since-last command. |
|
156 |
//if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0)) |
|
157 |
//{ |
|
158 |
// lin_vel_ = 0.0; |
|
159 |
// ang_vel_ = 0.0; |
|
160 |
//} |
|
161 |
|
|
162 |
// Update my position |
|
163 |
QPointF old_pos = pos_; |
|
164 |
|
|
165 |
/* Convert coordinate systens |
|
166 |
orient_ = std::fmod(orient_ + ang_vel_ * dt, 2*PI); |
|
167 |
pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt; |
|
168 |
pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt; |
|
169 |
*/ |
|
170 |
// TODO: what direction does orientation start at 0? |
|
171 |
// TODO: orient might have to become negative |
|
172 |
orient_ = std::fmod(orient_ - turnAngle_ * dt, 2*PI); |
|
173 |
// TODO: why add pi/2? is this just because they got the |
|
174 |
// sin/cos mixed up? |
|
175 |
//pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt; |
|
176 |
//pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt; |
|
177 |
|
|
178 |
pos_.rx() += std::cos(orient_) * lin_vel_ * dt; |
|
179 |
pos_.ry() += std::sin(orient_) * lin_vel_ * dt; |
|
180 |
|
|
181 |
|
|
182 |
// Find linear velocity |
|
183 |
//qreal lin_vel; |
|
184 |
// Find angular velocity |
|
185 |
//qreal ang_vel; |
|
186 |
|
|
187 |
// Clamp to screen size |
|
188 |
// TODO: change to use another image, to detect walls. |
|
189 |
if (pos_.x() < 0 || pos_.x() > canvas_width || |
|
190 |
pos_.y() < 0 || pos_.y() > canvas_height) { |
|
191 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(), |
|
192 |
pos_.y()); |
|
193 |
} |
|
194 |
|
|
195 |
pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), |
|
196 |
static_cast<double>(canvas_width))); |
|
197 |
pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), |
|
198 |
static_cast<double>(canvas_height))); |
|
199 |
|
|
200 |
// Publish pose of the turtle |
|
201 |
buggymsgs::Pose p; |
|
202 |
p.x = pos_.x(); |
|
203 |
p.y = canvas_height - pos_.y(); |
|
204 |
p.angle = orient_; |
|
205 |
p.linear_velocity = lin_vel_; |
|
206 |
//p.angular_velocity = ang_vel; |
|
207 |
pose_pub_.publish(p); |
|
208 |
|
|
209 |
// Figure out (and publish) the color underneath the turtle |
|
210 |
/*{ |
|
211 |
Color color; |
|
212 |
QRgb pixel = path_image.pixel((pos_ * meter_).toPoint()); |
|
213 |
color.r = qRed(pixel); |
|
214 |
color.g = qGreen(pixel); |
|
215 |
color.b = qBlue(pixel); |
|
216 |
color_pub_.publish(color); |
|
217 |
}*/ |
|
218 |
|
|
219 |
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(), |
|
220 |
pos_.x(), pos_.y(), orient_); |
|
221 |
|
|
222 |
if (orient_ != old_orient) { |
|
223 |
rotateImage(); |
|
224 |
modified = true; |
|
225 |
} |
|
226 |
if (pos_ != old_pos) { |
|
227 |
if (pen_on_) { |
|
228 |
path_painter.setPen(pen_); |
|
229 |
path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
|
230 |
} |
|
231 |
modified = true; |
|
232 |
} |
|
233 |
|
|
234 |
return modified; |
|
235 |
} |
|
236 |
|
|
237 |
void Turtle::paint(QPainter& painter) |
|
238 |
{ |
|
239 |
QPointF p = pos_ * meter_; |
|
240 |
p.rx() -= 0.5 * turtle_rotated_image_.width(); |
|
241 |
p.ry() -= 0.5 * turtle_rotated_image_.height(); |
|
242 |
painter.drawImage(p, turtle_rotated_image_); |
|
243 |
} |
|
248 | 244 |
|
249 | 245 |
} |
Also available in: Unified diff