Revision 066966cf
BuggyVis works
buggyvis/src/buggy.cpp | ||
---|---|---|
63 | 63 |
//Services |
64 | 64 |
set_pen_srv_ = nh_.advertiseService |
65 | 65 |
("set_pen", &Turtle::setPenCallback, this); |
66 |
teleport_relative_srv_ = nh_.advertiseService |
|
67 |
("teleport_relative", &Turtle::teleportRelativeCallback, this); |
|
68 |
teleport_absolute_srv_ = nh_.advertiseService |
|
69 |
("teleport_absolute", &Turtle::teleportAbsoluteCallback, this); |
|
66 |
//teleport_relative_srv_ = nh_.advertiseService
|
|
67 |
// ("teleport_relative", &Turtle::teleportRelativeCallback, this);
|
|
68 |
//teleport_absolute_srv_ = nh_.advertiseService
|
|
69 |
// ("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
|
|
70 | 70 |
|
71 | 71 |
meter_ = turtle_image_.height(); |
72 | 72 |
rotateImage(); |
... | ... | |
75 | 75 |
void Turtle::poseCallback(const buggymsgs::PoseConstPtr& pcp) |
76 | 76 |
{ |
77 | 77 |
last_command_time_ = ros::WallTime::now(); |
78 |
pose_ = pcp->pose;
|
|
78 |
pose_ = *pcp;
|
|
79 | 79 |
} |
80 | 80 |
|
81 | 81 |
bool Turtle::setPenCallback(buggysim::SetPen::Request& req, |
... | ... | |
108 | 108 |
* TODO: convert this to take just an angle. And include drag. |
109 | 109 |
* and include acceleration/joule dump |
110 | 110 |
*/ |
111 |
bool Turtle::update(double dt, |
|
112 |
QPainter& path_painter, |
|
111 |
bool Turtle::update(QPainter& path_painter, |
|
113 | 112 |
const QImage& path_image, |
114 | 113 |
qreal canvas_width, |
115 | 114 |
qreal canvas_height) |
... | ... | |
117 | 116 |
bool modified = false; |
118 | 117 |
qreal old_orient = orient_; |
119 | 118 |
|
120 |
// first process any teleportation requests, in order |
|
121 |
V_TeleportRequest::iterator it = teleport_requests_.begin(); |
|
122 |
V_TeleportRequest::iterator end = teleport_requests_.end(); |
|
123 |
for (; it != end; ++it) |
|
124 |
{ |
|
125 |
const TeleportRequest& req = *it; |
|
126 |
|
|
127 |
QPointF old_pos = pos_; |
|
128 |
if (req.relative) |
|
129 |
{ |
|
130 |
orient_ += req.theta; |
|
131 |
pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear; |
|
132 |
pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear; |
|
133 |
} |
|
134 |
else |
|
135 |
{ |
|
136 |
pos_.setX(req.pos.x()); |
|
137 |
pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y()))); |
|
138 |
orient_ = req.theta; |
|
139 |
} |
|
140 |
|
|
141 |
path_painter.setPen(pen_); |
|
142 |
path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
|
143 |
modified = true; |
|
144 |
} |
|
145 |
|
|
146 | 119 |
teleport_requests_.clear(); |
147 | 120 |
|
148 |
// Stop moving after 1 second since-last command. |
|
149 |
|
|
150 | 121 |
// Update my position |
151 | 122 |
QPointF old_pos = pos_; |
123 |
double dt = (ros::WallTime::now() - last_command_time_).toSec(); |
|
152 | 124 |
|
153 |
// TODO: what direction does orientation start at 0? |
|
154 |
// TODO: orient might have to become negative |
|
155 |
orient_ = std::fmod(orient_ - (turnAngle_ * dt), 2*PI); |
|
156 |
if (orient_ < 0.0) { |
|
157 |
orient_ += 2*PI; |
|
158 |
} |
|
125 |
//orient_ = std::fmod(orient_ - (turnAngle_ * dt), 2*PI); |
|
126 |
//if (orient_ < 0.0) { |
|
127 |
// orient_ += 2*PI; |
|
128 |
//} |
|
159 | 129 |
// TODO: why add pi/2? is this just because they got the |
160 | 130 |
// sin/cos mixed up? |
161 |
pos_.rx() += std::cos(orient_) * lin_vel_ * dt; |
|
162 |
pos_.ry() -= std::sin(orient_) * lin_vel_ * dt; |
|
131 |
//pos_.rx() += std::cos(orient_) * lin_vel_ * dt; |
|
132 |
//pos_.ry() -= std::sin(orient_) * lin_vel_ * dt; |
|
133 |
|
|
134 |
pos_.rx() = pose_.x; |
|
135 |
pos_.ry() = pose_.y; |
|
163 | 136 |
|
164 | 137 |
// Clamp to screen size |
165 | 138 |
// TODO: change to use another image, to detect walls. |
... | ... | |
175 | 148 |
static_cast<double>(canvas_height))); |
176 | 149 |
|
177 | 150 |
/** END POSITION UPDATE */ |
178 |
buggymsgs::Pose p; |
|
179 |
p.x = pos_.x(); |
|
180 |
p.y = canvas_height - pos_.y(); |
|
181 |
p.angle = orient_; |
|
182 |
p.linear_velocity = lin_vel_; |
|
183 |
pose_pub_.publish(p); |
|
151 |
//buggymsgs::Pose p;
|
|
152 |
//p.x = pos_.x();
|
|
153 |
//p.y = canvas_height - pos_.y();
|
|
154 |
//p.angle = orient_;
|
|
155 |
//p.linear_velocity = lin_vel_;
|
|
156 |
//pose_pub_.publish(p);
|
|
184 | 157 |
|
185 | 158 |
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", |
186 | 159 |
nh_.getNamespace().c_str(), |
Also available in: Unified diff