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(),
|