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/sim_frame.cpp | ||
|---|---|---|
| 26 | 26 |
SimFrame::SimFrame(wxWindow* parent) |
| 27 | 27 |
: wxFrame(parent, wxID_ANY, wxT("ScoutSim"), wxDefaultPosition,
|
| 28 | 28 |
wxSize(500, 500), wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER) |
| 29 |
, frame_count_(0)
|
|
| 30 |
, id_counter_(0)
|
|
| 29 |
, frame_count(0) |
|
| 30 |
, id_counter(0) |
|
| 31 | 31 |
{
|
| 32 | 32 |
srand(time(NULL)); |
| 33 | 33 |
|
| 34 |
update_timer_ = new wxTimer(this);
|
|
| 35 |
update_timer_->Start(16);
|
|
| 34 |
update_timer = new wxTimer(this); |
|
| 35 |
update_timer->Start(16); |
|
| 36 | 36 |
|
| 37 |
Connect(update_timer_->GetId(), wxEVT_TIMER,
|
|
| 37 |
Connect(update_timer->GetId(), wxEVT_TIMER, |
|
| 38 | 38 |
wxTimerEventHandler(SimFrame::onUpdate), NULL, this); |
| 39 | 39 |
Connect(wxEVT_PAINT, wxPaintEventHandler(SimFrame::onPaint), |
| 40 | 40 |
NULL, this); |
| 41 | 41 |
|
| 42 |
nh_.setParam("background_r", DEFAULT_BG_R);
|
|
| 43 |
nh_.setParam("background_g", DEFAULT_BG_G);
|
|
| 44 |
nh_.setParam("background_b", DEFAULT_BG_B);
|
|
| 42 |
nh.setParam("background_r", DEFAULT_BG_R);
|
|
| 43 |
nh.setParam("background_g", DEFAULT_BG_G);
|
|
| 44 |
nh.setParam("background_b", DEFAULT_BG_B);
|
|
| 45 | 45 |
|
| 46 | 46 |
std::string scouts[SCOUTSIM_NUM_SCOUTS] = |
| 47 | 47 |
{
|
| ... | ... | |
| 51 | 51 |
std::string images_path = ros::package::getPath("scoutsim")+"/images/";
|
| 52 | 52 |
for (size_t i = 0; i < SCOUTSIM_NUM_SCOUTS; ++i) |
| 53 | 53 |
{
|
| 54 |
scout_images_[i].LoadFile(
|
|
| 54 |
scout_images[i].LoadFile( |
|
| 55 | 55 |
wxString::FromAscii((images_path + scouts[i]).c_str())); |
| 56 |
scout_images_[i].SetMask(true);
|
|
| 57 |
scout_images_[i].SetMaskColour(255, 255, 255);
|
|
| 56 |
scout_images[i].SetMask(true); |
|
| 57 |
scout_images[i].SetMaskColour(255, 255, 255); |
|
| 58 | 58 |
} |
| 59 | 59 |
|
| 60 |
meter_ = scout_images_[0].GetHeight();
|
|
| 60 |
meter = scout_images[0].GetHeight();
|
|
| 61 | 61 |
|
| 62 |
path_bitmap_ = wxBitmap(GetSize().GetWidth(), GetSize().GetHeight());
|
|
| 63 |
path_dc_.SelectObject(path_bitmap_);
|
|
| 62 |
path_bitmap = wxBitmap(GetSize().GetWidth(), GetSize().GetHeight()); |
|
| 63 |
path_dc.SelectObject(path_bitmap);
|
|
| 64 | 64 |
clear(); |
| 65 | 65 |
|
| 66 |
clear_srv_ = nh_.advertiseService("clear",
|
|
| 66 |
clear_srv = nh.advertiseService("clear",
|
|
| 67 | 67 |
&SimFrame::clearCallback, this); |
| 68 |
reset_srv_ = nh_.advertiseService("reset",
|
|
| 68 |
reset_srv = nh.advertiseService("reset",
|
|
| 69 | 69 |
&SimFrame::resetCallback, this); |
| 70 |
spawn_srv_ = nh_.advertiseService("spawn",
|
|
| 70 |
spawn_srv = nh.advertiseService("spawn",
|
|
| 71 | 71 |
&SimFrame::spawnCallback, this); |
| 72 |
kill_srv_ = nh_.advertiseService("kill",
|
|
| 72 |
kill_srv = nh.advertiseService("kill",
|
|
| 73 | 73 |
&SimFrame::killCallback, this); |
| 74 | 74 |
|
| 75 | 75 |
ROS_INFO("Starting scoutsim with node name %s",
|
| 76 | 76 |
ros::this_node::getName().c_str()) ; |
| 77 | 77 |
|
| 78 |
width_in_meters_ = GetSize().GetWidth() / meter_;
|
|
| 79 |
height_in_meters_ = GetSize().GetHeight() / meter_;
|
|
| 80 |
spawnScout("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
|
|
| 78 |
width_in_meters = GetSize().GetWidth() / meter;
|
|
| 79 |
height_in_meters = GetSize().GetHeight() / meter;
|
|
| 80 |
spawnScout("", width_in_meters / 2.0, height_in_meters / 2.0, 0);
|
|
| 81 | 81 |
} |
| 82 | 82 |
|
| 83 | 83 |
SimFrame::~SimFrame() |
| 84 | 84 |
{
|
| 85 |
delete update_timer_;
|
|
| 85 |
delete update_timer; |
|
| 86 | 86 |
} |
| 87 | 87 |
|
| 88 | 88 |
bool SimFrame::spawnCallback(scoutsim::Spawn::Request& req, |
| ... | ... | |
| 103 | 103 |
bool SimFrame::killCallback(scoutsim::Kill::Request& req, |
| 104 | 104 |
scoutsim::Kill::Response&) |
| 105 | 105 |
{
|
| 106 |
M_Scout::iterator it = scouts_.find(req.name);
|
|
| 107 |
if (it == scouts_.end())
|
|
| 106 |
M_Scout::iterator it = scouts.find(req.name); |
|
| 107 |
if (it == scouts.end()) |
|
| 108 | 108 |
{
|
| 109 | 109 |
ROS_ERROR("Tried to kill scout [%s], which does not exist",
|
| 110 | 110 |
req.name.c_str()); |
| 111 | 111 |
return false; |
| 112 | 112 |
} |
| 113 | 113 |
|
| 114 |
scouts_.erase(it);
|
|
| 114 |
scouts.erase(it); |
|
| 115 | 115 |
|
| 116 | 116 |
return true; |
| 117 | 117 |
} |
| 118 | 118 |
|
| 119 | 119 |
bool SimFrame::hasScout(const std::string& name) |
| 120 | 120 |
{
|
| 121 |
return scouts_.find(name) != scouts_.end();
|
|
| 121 |
return scouts.find(name) != scouts.end();
|
|
| 122 | 122 |
} |
| 123 | 123 |
|
| 124 | 124 |
std::string SimFrame::spawnScout(const std::string& name, float x, |
| ... | ... | |
| 130 | 130 |
do |
| 131 | 131 |
{
|
| 132 | 132 |
std::stringstream ss; |
| 133 |
ss << "scout" << ++id_counter_;
|
|
| 133 |
ss << "scout" << ++id_counter; |
|
| 134 | 134 |
real_name = ss.str(); |
| 135 | 135 |
} while (hasScout(real_name)); |
| 136 | 136 |
} |
| ... | ... | |
| 143 | 143 |
} |
| 144 | 144 |
|
| 145 | 145 |
ScoutPtr t(new Scout(ros::NodeHandle(real_name), |
| 146 |
scout_images_[rand() % SCOUTSIM_NUM_SCOUTS],
|
|
| 146 |
scout_images[rand() % SCOUTSIM_NUM_SCOUTS], |
|
| 147 | 147 |
Vector2(x, y), angle)); |
| 148 |
scouts_[real_name] = t;
|
|
| 148 |
scouts[real_name] = t; |
|
| 149 | 149 |
|
| 150 | 150 |
ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",
|
| 151 | 151 |
real_name.c_str(), x, y, angle); |
| ... | ... | |
| 159 | 159 |
int g = DEFAULT_BG_G; |
| 160 | 160 |
int b = DEFAULT_BG_B; |
| 161 | 161 |
|
| 162 |
nh_.param("background_r", r, r);
|
|
| 163 |
nh_.param("background_g", g, g);
|
|
| 164 |
nh_.param("background_b", b, b);
|
|
| 162 |
nh.param("background_r", r, r);
|
|
| 163 |
nh.param("background_g", g, g);
|
|
| 164 |
nh.param("background_b", b, b);
|
|
| 165 | 165 |
|
| 166 |
path_dc_.SetBackground(wxBrush(wxColour(r, g, b)));
|
|
| 167 |
path_dc_.Clear();
|
|
| 166 |
path_dc.SetBackground(wxBrush(wxColour(r, g, b))); |
|
| 167 |
path_dc.Clear(); |
|
| 168 | 168 |
} |
| 169 | 169 |
|
| 170 | 170 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
| ... | ... | |
| 183 | 183 |
{
|
| 184 | 184 |
wxPaintDC dc(this); |
| 185 | 185 |
|
| 186 |
dc.DrawBitmap(path_bitmap_, 0, 0, true);
|
|
| 186 |
dc.DrawBitmap(path_bitmap, 0, 0, true); |
|
| 187 | 187 |
|
| 188 |
M_Scout::iterator it = scouts_.begin();
|
|
| 189 |
M_Scout::iterator end = scouts_.end();
|
|
| 188 |
M_Scout::iterator it = scouts.begin(); |
|
| 189 |
M_Scout::iterator end = scouts.end(); |
|
| 190 | 190 |
for (; it != end; ++it) |
| 191 | 191 |
{
|
| 192 | 192 |
it->second->paint(dc); |
| ... | ... | |
| 195 | 195 |
|
| 196 | 196 |
void SimFrame::updateScouts() |
| 197 | 197 |
{
|
| 198 |
if (last_scout_update_.isZero())
|
|
| 198 |
if (last_scout_update.isZero()) |
|
| 199 | 199 |
{
|
| 200 |
last_scout_update_ = ros::WallTime::now();
|
|
| 200 |
last_scout_update = ros::WallTime::now(); |
|
| 201 | 201 |
return; |
| 202 | 202 |
} |
| 203 | 203 |
|
| 204 |
if (frame_count_ % 3 == 0)
|
|
| 204 |
if (frame_count % 3 == 0) |
|
| 205 | 205 |
{
|
| 206 |
path_image_ = path_bitmap_.ConvertToImage();
|
|
| 206 |
path_image = path_bitmap.ConvertToImage();
|
|
| 207 | 207 |
Refresh(); |
| 208 | 208 |
} |
| 209 | 209 |
|
| 210 |
M_Scout::iterator it = scouts_.begin();
|
|
| 211 |
M_Scout::iterator end = scouts_.end();
|
|
| 210 |
M_Scout::iterator it = scouts.begin(); |
|
| 211 |
M_Scout::iterator end = scouts.end(); |
|
| 212 | 212 |
for (; it != end; ++it) |
| 213 | 213 |
{
|
| 214 |
it->second->update(0.016, path_dc_, path_image_,
|
|
| 215 |
path_dc_.GetBackground().GetColour(),
|
|
| 216 |
width_in_meters_, height_in_meters_);
|
|
| 214 |
it->second->update(0.016, path_dc, path_image,
|
|
| 215 |
path_dc.GetBackground().GetColour(), |
|
| 216 |
width_in_meters, height_in_meters);
|
|
| 217 | 217 |
} |
| 218 | 218 |
|
| 219 |
++frame_count_;
|
|
| 219 |
++frame_count; |
|
| 220 | 220 |
} |
| 221 | 221 |
|
| 222 | 222 |
|
| ... | ... | |
| 232 | 232 |
std_srvs::Empty::Response&) |
| 233 | 233 |
{
|
| 234 | 234 |
ROS_INFO("Resetting scoutsim.");
|
| 235 |
scouts_.clear();
|
|
| 236 |
id_counter_ = 0;
|
|
| 237 |
spawnScout("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0);
|
|
| 235 |
scouts.clear(); |
|
| 236 |
id_counter = 0; |
|
| 237 |
spawnScout("", width_in_meters / 2.0, height_in_meters / 2.0, 0);
|
|
| 238 | 238 |
clear(); |
| 239 | 239 |
return true; |
| 240 | 240 |
} |
Also available in: Unified diff