Revision 144137a1
| ID | 144137a12c1437f71d49b144279823dc836cb14b |
Got motor control working with scoutsim
Behaviors can now use the MotorControl class to change the speed of the motors in the simulator. The simulator correctly handles the command.
At the moment, the set_motors command is limited to 'scout1'. We should look into prefixes to specify which scout (scout1, scout2, etc) each behavior should command.
Also changed alex_behavior to be a good demonstration of the motors.
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 24 | 24 |
const wxImage& scout_image, |
| 25 | 25 |
const Vector2& pos, |
| 26 | 26 |
float orient) |
| 27 |
: n_(nh)
|
|
| 28 |
, scout_image_(scout_image)
|
|
| 29 |
, pos_(pos)
|
|
| 30 |
, orient_(orient)
|
|
| 31 |
, lin_vel_(0.0)
|
|
| 32 |
, ang_vel_(0.0)
|
|
| 33 |
, pen_on_(true)
|
|
| 34 |
, pen_(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
|
|
| 27 |
: node (nh)
|
|
| 28 |
, scout_image(scout_image) |
|
| 29 |
, pos(pos) |
|
| 30 |
, orient(orient) |
|
| 31 |
, lin_vel(0.0) |
|
| 32 |
, ang_vel(0.0) |
|
| 33 |
, pen_on(true) |
|
| 34 |
, pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) |
|
| 35 | 35 |
{
|
| 36 |
pen_.SetWidth(3); |
|
| 37 |
scout_ = wxBitmap(scout_image_); |
|
| 38 |
|
|
| 39 |
motors_sub_ = n_.subscribe("set_motors", 1, &Scout::setMotors, this);
|
|
| 40 |
|
|
| 41 |
/// @TODO take this out! |
|
| 42 |
velocity_sub_ = |
|
| 43 |
n_.subscribe("command_velocity",
|
|
| 44 |
1, |
|
| 45 |
&Scout::velocityCallback, this); |
|
| 46 |
pose_pub_ = n_.advertise<Pose>("pose", 1);
|
|
| 47 |
color_pub_ = n_.advertise<Color>("color_sensor", 1);
|
|
| 48 |
set_pen_srv_ = n_.advertiseService("set_pen",
|
|
| 36 |
pen.SetWidth(3); |
|
| 37 |
scout = wxBitmap(scout_image); |
|
| 38 |
|
|
| 39 |
motors_sub = node.subscribe("set_motors", 1, &Scout::setMotors, this);
|
|
| 40 |
|
|
| 41 |
pose_pub = node.advertise<Pose>("pose", 1);
|
|
| 42 |
color_pub = node.advertise<Color>("color_sensor", 1);
|
|
| 43 |
set_pen_srv = node.advertiseService("set_pen",
|
|
| 49 | 44 |
&Scout::setPenCallback, |
| 50 | 45 |
this); |
| 51 |
teleport_relative_srv_ =
|
|
| 52 |
n_.advertiseService("teleport_relative",
|
|
| 46 |
teleport_relative_srv = |
|
| 47 |
node.advertiseService("teleport_relative",
|
|
| 53 | 48 |
&Scout::teleportRelativeCallback, |
| 54 | 49 |
this); |
| 55 |
teleport_absolute_srv_ =
|
|
| 56 |
n_.advertiseService("teleport_absolute",
|
|
| 50 |
teleport_absolute_srv = |
|
| 51 |
node.advertiseService("teleport_absolute",
|
|
| 57 | 52 |
&Scout::teleportAbsoluteCallback, |
| 58 | 53 |
this); |
| 59 | 54 |
|
| 60 |
meter_ = scout_.GetHeight();
|
|
| 55 |
meter = scout.GetHeight();
|
|
| 61 | 56 |
} |
| 62 | 57 |
|
| 63 | 58 |
/** |
| ... | ... | |
| 66 | 61 |
*/ |
| 67 | 62 |
void Scout::setMotors(const motors::set_motors::ConstPtr& msg) |
| 68 | 63 |
{
|
| 69 |
last_command_time_ = ros::WallTime::now();
|
|
| 64 |
last_command_time = ros::WallTime::now(); |
|
| 70 | 65 |
|
| 71 | 66 |
if(msg->fl_set) |
| 72 | 67 |
{
|
| 73 |
motor_fl_speed_ = msg->fl_speed;
|
|
| 68 |
motor_fl_speed = msg->fl_speed; |
|
| 74 | 69 |
} |
| 75 | 70 |
if(msg->fr_set) |
| 76 | 71 |
{
|
| 77 |
motor_fr_speed_ = msg->fr_speed;
|
|
| 72 |
motor_fr_speed = msg->fr_speed; |
|
| 78 | 73 |
} |
| 79 | 74 |
if(msg->bl_set) |
| 80 | 75 |
{
|
| 81 |
motor_bl_speed_ = msg->bl_speed;
|
|
| 76 |
motor_bl_speed = msg->bl_speed; |
|
| 82 | 77 |
} |
| 83 | 78 |
if(msg->br_set) |
| 84 | 79 |
{
|
| 85 |
motor_br_speed_ = msg->br_speed;
|
|
| 80 |
motor_br_speed = msg->br_speed; |
|
| 86 | 81 |
} |
| 87 | 82 |
|
| 88 |
float l_speed = (float (motor_fl_speed_ + motor_bl_speed_)) / 2; |
|
| 89 |
float r_speed = (float (motor_fr_speed_ + motor_br_speed_)) / 2; |
|
| 90 |
|
|
| 91 |
/// @TODO Change this! |
|
| 92 |
lin_vel_ = (l_speed + r_speed) / 2; |
|
| 93 |
ang_vel_ = r_speed - l_speed; |
|
| 83 |
// Assume that the two motors on the same side will be set to |
|
| 84 |
// roughly the same speed. Does not account for slip conditions |
|
| 85 |
// when they are set to different speeds. |
|
| 86 |
float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2; |
|
| 87 |
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; |
|
| 94 | 88 |
|
| 95 |
} |
|
| 96 |
|
|
| 97 |
void Scout::velocityCallback(const VelocityConstPtr& vel) |
|
| 98 |
{
|
|
| 99 |
last_command_time_ = ros::WallTime::now(); |
|
| 100 |
lin_vel_ = vel->linear; |
|
| 101 |
ang_vel_ = vel->angular; |
|
| 89 |
// Set the linear and angular speeds |
|
| 90 |
lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2; |
|
| 91 |
ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed); |
|
| 102 | 92 |
} |
| 103 | 93 |
|
| 104 | 94 |
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, |
| 105 | 95 |
scoutsim::SetPen::Response&) |
| 106 | 96 |
{
|
| 107 |
pen_on_ = !req.off;
|
|
| 97 |
pen_on = !req.off; |
|
| 108 | 98 |
if (req.off) |
| 109 | 99 |
{
|
| 110 | 100 |
return true; |
| ... | ... | |
| 116 | 106 |
pen.SetWidth(req.width); |
| 117 | 107 |
} |
| 118 | 108 |
|
| 119 |
pen_ = pen;
|
|
| 109 |
pen = pen; |
|
| 120 | 110 |
return true; |
| 121 | 111 |
} |
| 122 | 112 |
|
| 113 |
/// @TODO remove |
|
| 123 | 114 |
bool Scout::teleportRelativeCallback(scoutsim::TeleportRelative::Request& req, |
| 124 | 115 |
scoutsim::TeleportRelative::Response&) |
| 125 | 116 |
{
|
| 126 |
teleport_requests_.push_back(TeleportRequest(0,
|
|
| 117 |
teleport_requests.push_back(TeleportRequest(0, |
|
| 127 | 118 |
0, |
| 128 | 119 |
req.angular, |
| 129 | 120 |
req.linear, |
| ... | ... | |
| 131 | 122 |
return true; |
| 132 | 123 |
} |
| 133 | 124 |
|
| 125 |
/// @TODO remove |
|
| 134 | 126 |
bool Scout::teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request& req, |
| 135 | 127 |
scoutsim::TeleportAbsolute::Response&) |
| 136 | 128 |
{
|
| 137 |
teleport_requests_.push_back(TeleportRequest(req.x,
|
|
| 129 |
teleport_requests.push_back(TeleportRequest(req.x, |
|
| 138 | 130 |
req.y, |
| 139 | 131 |
req.theta, |
| 140 | 132 |
0, |
| ... | ... | |
| 147 | 139 |
float canvas_width, float canvas_height) |
| 148 | 140 |
{
|
| 149 | 141 |
// first process any teleportation requests, in order |
| 150 |
V_TeleportRequest::iterator it = teleport_requests_.begin();
|
|
| 151 |
V_TeleportRequest::iterator end = teleport_requests_.end();
|
|
| 142 |
V_TeleportRequest::iterator it = teleport_requests.begin(); |
|
| 143 |
V_TeleportRequest::iterator end = teleport_requests.end(); |
|
| 152 | 144 |
for (; it != end; ++it) |
| 153 | 145 |
{
|
| 154 | 146 |
const TeleportRequest& req = *it; |
| 155 | 147 |
|
| 156 |
Vector2 old_pos = pos_;
|
|
| 148 |
Vector2 old_pos = pos; |
|
| 157 | 149 |
if (req.relative) |
| 158 | 150 |
{
|
| 159 |
orient_ += req.theta;
|
|
| 160 |
pos_.x += sin(orient_ + PI/2.0) * req.linear;
|
|
| 161 |
pos_.y += cos(orient_ + PI/2.0) * req.linear;
|
|
| 151 |
orient += req.theta; |
|
| 152 |
pos.x += sin(orient + PI/2.0) * req.linear;
|
|
| 153 |
pos.y += cos(orient + PI/2.0) * req.linear;
|
|
| 162 | 154 |
} |
| 163 | 155 |
else |
| 164 | 156 |
{
|
| 165 |
pos_.x = req.pos.x;
|
|
| 166 |
pos_.y = std::max(0.0f, canvas_height - req.pos.y);
|
|
| 167 |
orient_ = req.theta;
|
|
| 157 |
pos.x = req.pos.x; |
|
| 158 |
pos.y = std::max(0.0f, canvas_height - req.pos.y); |
|
| 159 |
orient = req.theta; |
|
| 168 | 160 |
} |
| 169 | 161 |
|
| 170 |
path_dc.SetPen(pen_);
|
|
| 171 |
path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,
|
|
| 172 |
old_pos.x * meter_, old_pos.y * meter_);
|
|
| 162 |
path_dc.SetPen(pen); |
|
| 163 |
path_dc.DrawLine(pos.x * meter, pos.y * meter,
|
|
| 164 |
old_pos.x * meter, old_pos.y * meter);
|
|
| 173 | 165 |
} |
| 174 | 166 |
|
| 175 |
teleport_requests_.clear();
|
|
| 167 |
teleport_requests.clear(); |
|
| 176 | 168 |
|
| 177 |
if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0))
|
|
| 169 |
if (ros::WallTime::now() - last_command_time > ros::WallDuration(1.0)) |
|
| 178 | 170 |
{
|
| 179 |
lin_vel_ = 0.0f;
|
|
| 180 |
ang_vel_ = 0.0f;
|
|
| 171 |
lin_vel = 0.0f; |
|
| 172 |
ang_vel = 0.0f; |
|
| 181 | 173 |
} |
| 182 | 174 |
|
| 183 |
Vector2 old_pos = pos_;
|
|
| 175 |
Vector2 old_pos = pos; |
|
| 184 | 176 |
|
| 185 |
orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);
|
|
| 186 |
pos_.x += sin(orient_ + PI/2.0) * lin_vel_ * dt;
|
|
| 187 |
pos_.y += cos(orient_ + PI/2.0) * lin_vel_ * dt;
|
|
| 177 |
orient = fmod(orient + ang_vel * dt, 2*PI);
|
|
| 178 |
pos.x += sin(orient + PI/2.0) * lin_vel * dt;
|
|
| 179 |
pos.y += cos(orient + PI/2.0) * lin_vel * dt;
|
|
| 188 | 180 |
|
| 189 | 181 |
// Clamp to screen size |
| 190 |
if (pos_.x < 0 || pos_.x >= canvas_width
|
|
| 191 |
|| pos_.y < 0 || pos_.y >= canvas_height)
|
|
| 182 |
if (pos.x < 0 || pos.x >= canvas_width
|
|
| 183 |
|| pos.y < 0 || pos.y >= canvas_height)
|
|
| 192 | 184 |
{
|
| 193 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x, pos_.y);
|
|
| 185 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
|
|
| 194 | 186 |
} |
| 195 | 187 |
|
| 196 |
pos_.x = std::min(std::max(pos_.x, 0.0f), canvas_width);
|
|
| 197 |
pos_.y = std::min(std::max(pos_.y, 0.0f), canvas_height);
|
|
| 188 |
pos.x = std::min(std::max(pos.x, 0.0f), canvas_width);
|
|
| 189 |
pos.y = std::min(std::max(pos.y, 0.0f), canvas_height);
|
|
| 198 | 190 |
|
| 199 |
int canvas_x = pos_.x * meter_;
|
|
| 200 |
int canvas_y = pos_.y * meter_;
|
|
| 191 |
int canvas_x = pos.x * meter;
|
|
| 192 |
int canvas_y = pos.y * meter;
|
|
| 201 | 193 |
|
| 202 | 194 |
{
|
| 203 | 195 |
wxImage rotated_image = |
| 204 |
scout_image_.Rotate(orient_ - PI/2.0,
|
|
| 205 |
wxPoint(scout_image_.GetWidth() / 2,
|
|
| 206 |
scout_image_.GetHeight() / 2),
|
|
| 196 |
scout_image.Rotate(orient - PI/2.0,
|
|
| 197 |
wxPoint(scout_image.GetWidth() / 2,
|
|
| 198 |
scout_image.GetHeight() / 2), |
|
| 207 | 199 |
false); |
| 208 | 200 |
|
| 209 | 201 |
for (int y = 0; y < rotated_image.GetHeight(); ++y) |
| ... | ... | |
| 219 | 211 |
} |
| 220 | 212 |
} |
| 221 | 213 |
|
| 222 |
scout_ = wxBitmap(rotated_image);
|
|
| 214 |
scout = wxBitmap(rotated_image); |
|
| 223 | 215 |
} |
| 224 | 216 |
|
| 225 | 217 |
Pose p; |
| 226 |
p.x = pos_.x;
|
|
| 227 |
p.y = canvas_height - pos_.y;
|
|
| 228 |
p.theta = orient_;
|
|
| 229 |
p.linear_velocity = lin_vel_;
|
|
| 230 |
p.angular_velocity = ang_vel_;
|
|
| 231 |
pose_pub_.publish(p);
|
|
| 218 |
p.x = pos.x; |
|
| 219 |
p.y = canvas_height - pos.y; |
|
| 220 |
p.theta = orient; |
|
| 221 |
p.linear_velocity = lin_vel; |
|
| 222 |
p.angular_velocity = ang_vel; |
|
| 223 |
pose_pub.publish(p); |
|
| 232 | 224 |
|
| 233 | 225 |
// Figure out (and publish) the color underneath the scout |
| 234 | 226 |
{
|
| 235 |
wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());
|
|
| 227 |
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
|
|
| 236 | 228 |
Color color; |
| 237 | 229 |
color.r = path_image.GetRed(canvas_x, canvas_y); |
| 238 | 230 |
color.g = path_image.GetGreen(canvas_x, canvas_y); |
| 239 | 231 |
color.b = path_image.GetBlue(canvas_x, canvas_y); |
| 240 |
color_pub_.publish(color);
|
|
| 232 |
color_pub.publish(color); |
|
| 241 | 233 |
} |
| 242 | 234 |
|
| 243 | 235 |
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
|
| 244 |
n_.getNamespace().c_str(), pos_.x, pos_.y, orient_);
|
|
| 236 |
node.getNamespace().c_str(), pos.x, pos.y, orient);
|
|
| 245 | 237 |
|
| 246 |
if (pen_on_)
|
|
| 238 |
if (pen_on) |
|
| 247 | 239 |
{
|
| 248 |
if (pos_ != old_pos)
|
|
| 240 |
if (pos != old_pos) |
|
| 249 | 241 |
{
|
| 250 |
path_dc.SetPen(pen_);
|
|
| 251 |
path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_,
|
|
| 252 |
old_pos.x * meter_, old_pos.y * meter_);
|
|
| 242 |
path_dc.SetPen(pen); |
|
| 243 |
path_dc.DrawLine(pos.x * meter, pos.y * meter,
|
|
| 244 |
old_pos.x * meter, old_pos.y * meter);
|
|
| 253 | 245 |
} |
| 254 | 246 |
} |
| 255 | 247 |
} |
| 256 | 248 |
|
| 257 | 249 |
void Scout::paint(wxDC& dc) |
| 258 | 250 |
{
|
| 259 |
wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight());
|
|
| 260 |
dc.DrawBitmap(scout_, pos_.x * meter_ - (scout_size.GetWidth() / 2),
|
|
| 261 |
pos_.y * meter_ - (scout_size.GetHeight() / 2), true);
|
|
| 251 |
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
|
|
| 252 |
dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
|
|
| 253 |
pos.y * meter - (scout_size.GetHeight() / 2), true);
|
|
| 262 | 254 |
} |
| 263 | 255 |
|
| 264 | 256 |
} |
Also available in: Unified diff