robobuggy / buggysim / src / adelaide.cpp @ 1efba872
History | View | Annotate | Download (7.57 KB)
1 | 73279b33 | Tahm | // This file will contain the details specific to this revision of the
|
---|---|---|---|
2 | // simulator. Most other upgrades (painting, maps, spawning)
|
||
3 | 08be08ec | Tahm | // will be in AbstractBuggy.
|
4 | // This file will focus on being a physics simulation engine.
|
||
5 | 1efba872 | Tahm | /*
|
6 | * Copyright (c) 2009, Willow Garage, Inc.
|
||
7 | * All rights reserved.
|
||
8 | *
|
||
9 | * Redistribution and use in source and binary forms, with or without
|
||
10 | * modification, are permitted provided that the following conditions are met:
|
||
11 | *
|
||
12 | * * Redistributions of source code must retain the above copyright
|
||
13 | * notice, this list of conditions and the following disclaimer.
|
||
14 | * * Redistributions in binary form must reproduce the above copyright
|
||
15 | * notice, this list of conditions and the following disclaimer in the
|
||
16 | * documentation and/or other materials provided with the distribution.
|
||
17 | * * Neither the name of the Willow Garage, Inc. nor the names of its
|
||
18 | * contributors may be used to endorse or promote products derived from
|
||
19 | * this software without specific prior written permission.
|
||
20 | *
|
||
21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||
22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||
23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||
24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||
25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||
26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||
27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||
28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||
29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||
30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
31 | * POSSIBILITY OF SUCH DAMAGE.
|
||
32 | */
|
||
33 | |||
34 | #include "buggysim/buggy.h" |
||
35 | |||
36 | #include <QColor> |
||
37 | #include <QRgb> |
||
38 | |||
39 | #define DEFAULT_PEN_R 0xb3 |
||
40 | #define DEFAULT_PEN_G 0xb8 |
||
41 | #define DEFAULT_PEN_B 0xff |
||
42 | |||
43 | namespace buggysim
|
||
44 | { |
||
45 | class Adelaide: public AbstractBuggy |
||
46 | { |
||
47 | public:
|
||
48 | ros::NodeHandle nh_; |
||
49 | |||
50 | // Linear velocity, measured in meters/sec
|
||
51 | qreal lin_vel_; |
||
52 | //qreal ang_vel_;
|
||
53 | qreal turnAngle_; |
||
54 | |||
55 | ros::Subscriber angle_sub_; |
||
56 | ros::Publisher pose_pub_; |
||
57 | |||
58 | float meter_;
|
||
59 | |||
60 | //void turnAngleCallback(const buggymsgs::DirectionConstPtr& dir);
|
||
61 | |||
62 | Adelaide(const ros::NodeHandle& nh, const QImage& turtle_image, |
||
63 | const QPointF& pos, float orient) |
||
64 | : AbstractBuggy(nh, turtle_image, pos, orient) |
||
65 | , lin_vel_(1.0) |
||
66 | //, ang_vel_(0.0)
|
||
67 | , turnAngle_(0.0) // turn angle is stored in degrees |
||
68 | //, pen_on_(true)
|
||
69 | //, pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
|
||
70 | { |
||
71 | ROS_INFO("Namespace in childbuggy, unresolved: %s", nh_.getUnresolvedNamespace().data());
|
||
72 | ROS_INFO("Namespace in childbuggy: %s", nh_.getNamespace().data());
|
||
73 | //ROS_INFO("Namespace in childbuggy: %s", nh_.remapName());
|
||
74 | //ROS_INFO("Namespace in childbuggy, of arg: %s", nh.getNamespace().data());
|
||
75 | |||
76 | // TODO: fix this hack.
|
||
77 | angle_sub_ = nh_.subscribe |
||
78 | ("/buggy1/command_turnAngle", 1, &Adelaide::turnAngleCallback, this); |
||
79 | pose_pub_ = nh_.advertise<buggymsgs::Pose>("/buggy1/pose", 1); |
||
80 | |||
81 | |||
82 | } |
||
83 | |||
84 | |||
85 | void turnAngleCallback(const buggymsgs::DirectionConstPtr& dir) |
||
86 | { |
||
87 | last_command_time_ = ros::WallTime::now(); |
||
88 | turnAngle_ = dir->turnAngle; |
||
89 | //lin_vel_ = vel->linear;
|
||
90 | //ang_vel_ = vel->angular;
|
||
91 | } |
||
92 | |||
93 | |||
94 | /**
|
||
95 | * TODO: convert this to take just an angle. And include drag.
|
||
96 | * and include acceleration/joule dump
|
||
97 | */
|
||
98 | bool update(double dt, QPainter& path_painter, const QImage& path_image, |
||
99 | qreal canvas_width, qreal canvas_height) |
||
100 | { |
||
101 | qreal old_orient = orient_; |
||
102 | bool modified = false; |
||
103 | //qreal old_orient = orient_;
|
||
104 | |||
105 | // first process any teleportation requests, in order
|
||
106 | /*V_TeleportRequest::iterator it = teleport_requests_.begin();
|
||
107 | V_TeleportRequest::iterator end = teleport_requests_.end();
|
||
108 | for (; it != end; ++it) {
|
||
109 | const TeleportRequest& req = *it;
|
||
110 | |||
111 | QPointF old_pos = pos_;
|
||
112 | if (req.relative) {
|
||
113 | orient_ += req.theta;
|
||
114 | pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear;
|
||
115 | pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear;
|
||
116 | } else {
|
||
117 | pos_.setX(req.pos.x());
|
||
118 | pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));
|
||
119 | orient_ = req.theta;
|
||
120 | }
|
||
121 | |||
122 | path_painter.setPen(pen_);
|
||
123 | path_painter.drawLine(pos_ * meter_, old_pos * meter_);
|
||
124 | modified = true;
|
||
125 | }
|
||
126 | |||
127 | teleport_requests_.clear();
|
||
128 | */
|
||
129 | // Stop moving after 1 second since-last command.
|
||
130 | //if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))
|
||
131 | //{
|
||
132 | // lin_vel_ = 0.0;
|
||
133 | // ang_vel_ = 0.0;
|
||
134 | //}
|
||
135 | |||
136 | // Update my position
|
||
137 | QPointF old_pos = pos_; |
||
138 | |||
139 | /* Convert coordinate systens
|
||
140 | orient_ = std::fmod(orient_ + ang_vel_ * dt, 2*PI);
|
||
141 | pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;
|
||
142 | pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;
|
||
143 | */
|
||
144 | // TODO: what direction does orientation start at 0?
|
||
145 | // TODO: orient might have to become negative
|
||
146 | //orient_ = std::fmod(orient_ + ((turnAngle_/2*PI) * dt), 2*PI);
|
||
147 | orient_ = std::fmod(orient_ + ((-turnAngle_/(2*PI)) * dt), 2*PI); |
||
148 | // TODO: why add pi/2? is this just because they got the
|
||
149 | // sin/cos mixed up?
|
||
150 | //pos_.rx() += std::sin(orient_ + PI/2.0) * lin_vel_ * dt;
|
||
151 | //pos_.ry() += std::cos(orient_ + PI/2.0) * lin_vel_ * dt;
|
||
152 | |||
153 | pos_.rx() += std::cos(orient_) * lin_vel_ * dt; |
||
154 | pos_.ry() -= std::sin(orient_) * lin_vel_ * dt; |
||
155 | |||
156 | |||
157 | // Find linear velocity
|
||
158 | //qreal lin_vel;
|
||
159 | // Find angular velocity
|
||
160 | //qreal ang_vel;
|
||
161 | |||
162 | // Clamp to screen size
|
||
163 | // TODO: change to use another image, to detect walls.
|
||
164 | if (pos_.x() < 0 || pos_.x() > canvas_width || |
||
165 | pos_.y() < 0 || pos_.y() > canvas_height) {
|
||
166 | ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x(),
|
||
167 | pos_.y()); |
||
168 | } |
||
169 | |||
170 | pos_.setX(std::min(std::max(static_cast<double>(pos_.x()), 0.0), |
||
171 | static_cast<double>(canvas_width))); |
||
172 | pos_.setY(std::min(std::max(static_cast<double>(pos_.y()), 0.0), |
||
173 | static_cast<double>(canvas_height))); |
||
174 | |||
175 | // Publish pose of the turtle
|
||
176 | buggymsgs::Pose p; |
||
177 | p.x = pos_.x(); |
||
178 | p.y = canvas_height - pos_.y(); |
||
179 | p.angle = orient_; |
||
180 | p.linear_velocity = lin_vel_; |
||
181 | //p.angular_velocity = ang_vel;
|
||
182 | pose_pub_.publish(p); |
||
183 | |||
184 | // Figure out (and publish) the color underneath the turtle
|
||
185 | /*{
|
||
186 | Color color;
|
||
187 | QRgb pixel = path_image.pixel((pos_ * meter_).toPoint());
|
||
188 | color.r = qRed(pixel);
|
||
189 | color.g = qGreen(pixel);
|
||
190 | color.b = qBlue(pixel);
|
||
191 | color_pub_.publish(color);
|
||
192 | }*/
|
||
193 | |||
194 | ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", nh_.getNamespace().c_str(),
|
||
195 | pos_.x(), pos_.y(), orient_); |
||
196 | |||
197 | if (orient_ != old_orient) {
|
||
198 | rotateImage(); |
||
199 | modified = true;
|
||
200 | } |
||
201 | if (pos_ != old_pos) {
|
||
202 | if (pen_on_) {
|
||
203 | path_painter.setPen(pen_); |
||
204 | path_painter.drawLine(pos_ * meter_, old_pos * meter_); |
||
205 | } |
||
206 | modified = true;
|
||
207 | } |
||
208 | |||
209 | return modified;
|
||
210 | } |
||
211 | |||
212 | |||
213 | }; |
||
214 | } |