Revision af0d9743
Added template linesensors to scoutsim
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 53 | 53 |
#define DEFAULT_PEN_G 0xb8 |
| 54 | 54 |
#define DEFAULT_PEN_B 0xff |
| 55 | 55 |
|
| 56 |
using namespace std; |
|
| 57 |
|
|
| 56 | 58 |
namespace scoutsim |
| 57 | 59 |
{
|
| 58 | 60 |
Scout::Scout(const ros::NodeHandle& nh, |
| ... | ... | |
| 89 | 91 |
&Scout::query_encoders_callback, |
| 90 | 92 |
this); |
| 91 | 93 |
|
| 94 |
query_linesensor_srv = |
|
| 95 |
node.advertiseService("query_linesensor",
|
|
| 96 |
&Scout::query_linesensor_callback, |
|
| 97 |
this); |
|
| 98 |
|
|
| 92 | 99 |
meter = scout.GetHeight(); |
| 93 | 100 |
} |
| 94 | 101 |
|
| ... | ... | |
| 156 | 163 |
return true; |
| 157 | 164 |
} |
| 158 | 165 |
|
| 166 |
bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&, |
|
| 167 |
linesensor::query_linesensor::Response& res) |
|
| 168 |
{
|
|
| 169 |
vector<unsigned int> readings; |
|
| 170 |
|
|
| 171 |
for (unsigned int i = 0; i < 8; i++) |
|
| 172 |
{
|
|
| 173 |
readings.push_back(8 - i); |
|
| 174 |
} |
|
| 175 |
|
|
| 176 |
res.readings = readings; |
|
| 177 |
|
|
| 178 |
return true; |
|
| 179 |
} |
|
| 180 |
|
|
| 159 | 181 |
/// Sends back the position of this scout so scoutsim can save |
| 160 | 182 |
/// the world state |
| 161 | 183 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| ... | ... | |
| 193 | 215 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
|
| 194 | 216 |
} |
| 195 | 217 |
|
| 196 |
pos.x = std::min(std::max(pos.x, 0.0f), state.canvas_width);
|
|
| 197 |
pos.y = std::min(std::max(pos.y, 0.0f), state.canvas_height);
|
|
| 218 |
pos.x = min(max(pos.x, 0.0f), state.canvas_width);
|
|
| 219 |
pos.y = min(max(pos.y, 0.0f), state.canvas_height);
|
|
| 198 | 220 |
|
| 199 | 221 |
int canvas_x = pos.x * meter; |
| 200 | 222 |
int canvas_y = pos.y * meter; |
Also available in: Unified diff