Revision ade1b7f9
| ID | ade1b7f9e6aee15cde7890ca6330f9cd57e118b5 |
Basic line sensor readings!
| scout/libscout/src/LinesensorControl.cpp | ||
|---|---|---|
| 64 | 64 |
|
| 65 | 65 |
for (int i = 0; i < 8; i++) |
| 66 | 66 |
{
|
| 67 |
cout << "[" << i << "]: " << srv.response.readings[i] << ", " << endl;
|
|
| 67 |
cout << "[" << i << "]: " << srv.response.readings[i] << ", "; |
|
| 68 | 68 |
} |
| 69 |
|
|
| 70 |
cout << endl; |
|
| 69 | 71 |
} |
| 70 | 72 |
|
| scout/libscout/src/behaviors/sim_line.cpp | ||
|---|---|---|
| 29 | 29 |
{
|
| 30 | 30 |
while(ok()) |
| 31 | 31 |
{
|
| 32 |
motors->set_sides(40, 80, MOTOR_ABSOLUTE);
|
|
| 32 |
motors->set_sides(10, 20, MOTOR_ABSOLUTE);
|
|
| 33 | 33 |
|
| 34 | 34 |
linesensor->query(); |
| 35 | 35 |
|
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 96 | 96 |
&Scout::query_linesensor_callback, |
| 97 | 97 |
this); |
| 98 | 98 |
|
| 99 |
for (unsigned int i = 0; i < NUM_LINESENSORS; i++) |
|
| 100 |
{
|
|
| 101 |
linesensor_readings.push_back(0); |
|
| 102 |
} |
|
| 103 |
|
|
| 99 | 104 |
meter = scout.GetHeight(); |
| 100 | 105 |
} |
| 101 | 106 |
|
| ... | ... | |
| 166 | 171 |
bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&, |
| 167 | 172 |
linesensor::query_linesensor::Response& res) |
| 168 | 173 |
{
|
| 169 |
vector<unsigned int> readings; |
|
| 174 |
res.readings = linesensor_readings; |
|
| 175 |
|
|
| 176 |
return true; |
|
| 177 |
} |
|
| 170 | 178 |
|
| 171 |
for (unsigned int i = 0; i < 8; i++) |
|
| 179 |
// Scale to linesensor value |
|
| 180 |
unsigned int Scout::rgb_to_linesensor_val(unsigned char r, |
|
| 181 |
unsigned char g, |
|
| 182 |
unsigned char b) |
|
| 183 |
{
|
|
| 184 |
// Should be 0 to 255 |
|
| 185 |
unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3; |
|
| 186 |
|
|
| 187 |
/// @todo Convert to the proper range |
|
| 188 |
return 255 - grey; |
|
| 189 |
} |
|
| 190 |
|
|
| 191 |
void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta) |
|
| 192 |
{
|
|
| 193 |
linesensor_readings.clear(); |
|
| 194 |
|
|
| 195 |
double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1); |
|
| 196 |
for (int s = 0; s < NUM_LINESENSORS; s++) |
|
| 172 | 197 |
{
|
| 173 |
readings.push_back(8 - i); |
|
| 174 |
} |
|
| 198 |
double offset = -(scout_image.GetWidth() / 2) + s * spacing; |
|
| 175 | 199 |
|
| 176 |
res.readings = readings; |
|
| 200 |
int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta)); |
|
| 201 |
int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta)); |
|
| 177 | 202 |
|
| 178 |
return true; |
|
| 203 |
/* |
|
| 204 |
if (s == 0) |
|
| 205 |
{
|
|
| 206 |
cout << "s: " << s << ", offset: " << offset << ", scout: (" << x <<
|
|
| 207 |
", " << y << "), sensor: (" << sensor_x <<
|
|
| 208 |
", " << sensor_y << ")" << endl; |
|
| 209 |
} |
|
| 210 |
*/ |
|
| 211 |
|
|
| 212 |
unsigned char r = lines_image.GetRed(sensor_x, sensor_y); |
|
| 213 |
unsigned char g = lines_image.GetGreen(sensor_x, sensor_y); |
|
| 214 |
unsigned char b = lines_image.GetBlue(sensor_x, sensor_y); |
|
| 215 |
|
|
| 216 |
unsigned int reading = rgb_to_linesensor_val(r, g, b); |
|
| 217 |
|
|
| 218 |
linesensor_readings.push_back(reading); |
|
| 219 |
} |
|
| 179 | 220 |
} |
| 180 | 221 |
|
| 181 | 222 |
/// Sends back the position of this scout so scoutsim can save |
| 182 | 223 |
/// the world state |
| 183 | 224 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| 184 | 225 |
const wxImage& path_image, |
| 226 |
const wxImage& lines_image, |
|
| 185 | 227 |
wxColour background_color, |
| 186 | 228 |
world_state state) |
| 187 | 229 |
{
|
| ... | ... | |
| 225 | 267 |
wxImage rotated_image = |
| 226 | 268 |
scout_image.Rotate(orient - PI/2.0, |
| 227 | 269 |
wxPoint(scout_image.GetWidth() / 2, |
| 228 |
scout_image.GetHeight() / 2),
|
|
| 270 |
scout_image.GetHeight() / 2), |
|
| 229 | 271 |
false); |
| 230 | 272 |
|
| 231 | 273 |
for (int y = 0; y < rotated_image.GetHeight(); ++y) |
| ... | ... | |
| 252 | 294 |
p.angular_velocity = ang_vel; |
| 253 | 295 |
pose_pub.publish(p); |
| 254 | 296 |
|
| 297 |
update_linesensor(lines_image, canvas_x, canvas_y, p.theta); |
|
| 298 |
|
|
| 255 | 299 |
// Figure out (and publish) the color underneath the scout |
| 256 | 300 |
{
|
| 257 | 301 |
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); |
| scout/scoutsim/src/scout.h | ||
|---|---|---|
| 73 | 73 |
/// The scale factor so the speed of scout robots is reasonable for the sim |
| 74 | 74 |
#define SPEED_SCALE_FACTOR 0.05 |
| 75 | 75 |
|
| 76 |
#define NUM_LINESENSORS 8 |
|
| 77 |
|
|
| 78 |
// Distance, pixels, from center of robot to the linesensors. |
|
| 79 |
#define LNSNSR_D 20 |
|
| 80 |
|
|
| 76 | 81 |
namespace scoutsim |
| 77 | 82 |
{
|
| 78 | 83 |
struct Vector2 |
| ... | ... | |
| 109 | 114 |
|
| 110 | 115 |
geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc, |
| 111 | 116 |
const wxImage& path_image, |
| 117 |
const wxImage& lines_image, |
|
| 112 | 118 |
wxColour background_color, |
| 113 | 119 |
world_state state); |
| 114 | 120 |
void paint(wxDC& dc); |
| ... | ... | |
| 122 | 128 |
encoders::query_encoders::Response&); |
| 123 | 129 |
bool query_linesensor_callback(linesensor::query_linesensor::Request&, |
| 124 | 130 |
linesensor::query_linesensor::Response&); |
| 131 |
unsigned int rgb_to_linesensor_val(unsigned char r, |
|
| 132 |
unsigned char g, |
|
| 133 |
unsigned char b); |
|
| 134 |
void update_linesensor(const wxImage& lines_image, int x, int y, double theta); |
|
| 125 | 135 |
|
| 126 | 136 |
ros::NodeHandle node; |
| 127 | 137 |
|
| ... | ... | |
| 145 | 155 |
unsigned int bl_ticks; |
| 146 | 156 |
unsigned int br_ticks; |
| 147 | 157 |
|
| 158 |
// A vector of the 8 linesensor readings |
|
| 159 |
std::vector<unsigned int> linesensor_readings; |
|
| 160 |
|
|
| 148 | 161 |
// Each scout has a unique id number, which is also displayed on its image. |
| 149 | 162 |
int scout_id; |
| 150 | 163 |
|
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 92 | 92 |
/// @todo This should change. |
| 93 | 93 |
meter = scout_images[0].GetHeight(); |
| 94 | 94 |
|
| 95 |
base_map_name = map_name; |
|
| 96 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" + map_name + ".bmp";
|
|
| 95 |
map_base_name = ros::package::getPath("scoutsim") + "/maps/" +
|
|
| 96 |
map_name + ".bmp"; |
|
| 97 |
map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
|
|
| 98 |
map_name + "_lines.bmp"; |
|
| 99 |
display_map_name = map_base_name; |
|
| 100 |
|
|
| 101 |
wxBitmap lines_bitmap; |
|
| 102 |
lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())); |
|
| 103 |
lines_image = lines_bitmap.ConvertToImage(); |
|
| 104 |
|
|
| 97 | 105 |
clear(); |
| 98 | 106 |
|
| 99 | 107 |
clear_srv = nh.advertiseService("clear",
|
| ... | ... | |
| 227 | 235 |
|
| 228 | 236 |
void SimFrame::showMap(wxCommandEvent& WXUNUSED(event)) |
| 229 | 237 |
{
|
| 230 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
|
|
| 231 |
base_map_name + ".bmp"; |
|
| 238 |
display_map_name = map_base_name; |
|
| 232 | 239 |
clear(); |
| 233 | 240 |
} |
| 234 | 241 |
|
| 235 | 242 |
void SimFrame::showLines(wxCommandEvent& WXUNUSED(event)) |
| 236 | 243 |
{
|
| 237 |
display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
|
|
| 238 |
base_map_name + "_lines.bmp"; |
|
| 244 |
display_map_name = map_lines_name; |
|
| 239 | 245 |
clear(); |
| 240 | 246 |
} |
| 241 | 247 |
|
| ... | ... | |
| 298 | 304 |
|
| 299 | 305 |
for (; it != end; ++it) |
| 300 | 306 |
{
|
| 301 |
it->second->update(0.016, path_dc, path_image, |
|
| 307 |
it->second->update(0.016, path_dc, path_image, lines_image,
|
|
| 302 | 308 |
path_dc.GetBackground().GetColour(), |
| 303 | 309 |
state); |
| 304 | 310 |
} |
| scout/scoutsim/src/sim_frame.h | ||
|---|---|---|
| 103 | 103 |
wxTimer* update_timer; |
| 104 | 104 |
wxBitmap path_bitmap; |
| 105 | 105 |
wxImage path_image; |
| 106 |
wxImage lines_image; |
|
| 106 | 107 |
wxMemoryDC path_dc; |
| 107 | 108 |
|
| 108 | 109 |
uint64_t frame_count; |
| ... | ... | |
| 124 | 125 |
float width_in_meters; |
| 125 | 126 |
float height_in_meters; |
| 126 | 127 |
|
| 127 |
std::string base_map_name; |
|
| 128 |
std::string map_base_name; |
|
| 129 |
std::string map_lines_name; |
|
| 128 | 130 |
std::string display_map_name; |
| 129 | 131 |
}; |
| 130 | 132 |
|
Also available in: Unified diff