Revision a2e6bd4c
Added sonar, though it looks buggy.
Use sonar_viz to continue debugging and make sonar work!
scout/scoutsim/src/scout.h | ||
---|---|---|
55 | 55 |
#include <motors/set_motors.h> |
56 | 56 |
#include <encoders/query_encoders.h> |
57 | 57 |
#include <linesensor/query_linesensor.h> |
58 |
#include <sonar/sonar_distance.h> |
|
58 | 59 |
|
59 | 60 |
#include <scoutsim/Pose.h> |
60 | 61 |
#include <scoutsim/SetPen.h> |
... | ... | |
65 | 66 |
#include <wx/wx.h> |
66 | 67 |
|
67 | 68 |
#include "scoutsim_internal.h" |
68 |
|
|
69 |
//#include "scout_constants.h" |
|
69 |
#include "scout_constants.h" |
|
70 | 70 |
|
71 | 71 |
#define PI 3.14159265 |
72 | 72 |
|
... | ... | |
115 | 115 |
geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc, |
116 | 116 |
const wxImage& path_image, |
117 | 117 |
const wxImage& lines_image, |
118 |
const wxImage& walls_image, |
|
118 | 119 |
wxColour background_color, |
119 | 120 |
world_state state); |
120 | 121 |
void paint(wxDC& dc); |
... | ... | |
128 | 129 |
encoders::query_encoders::Response&); |
129 | 130 |
bool query_linesensor_callback(linesensor::query_linesensor::Request&, |
130 | 131 |
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); |
|
132 |
unsigned int rgb_to_grey(unsigned char r, |
|
133 |
unsigned char g, |
|
134 |
unsigned char b); |
|
135 |
unsigned int trace_sonar(const wxImage& walls_image, int x, int y, |
|
136 |
double robot_theta, int sonar_pos); |
|
137 |
void update_sonar(const wxImage& walls_image, int x, int y, |
|
138 |
double robot_theta); |
|
139 |
void update_linesensor(const wxImage& lines_image, int x, int y, |
|
140 |
double theta); |
|
135 | 141 |
|
136 | 142 |
ros::NodeHandle node; |
137 | 143 |
|
... | ... | |
155 | 161 |
unsigned int bl_ticks; |
156 | 162 |
unsigned int br_ticks; |
157 | 163 |
|
164 |
int sonar_position; |
|
165 |
int sonar_stop_l; |
|
166 |
int sonar_stop_r; |
|
167 |
int sonar_direction; |
|
168 |
// The last time the sonar changed its position. |
|
169 |
ros::Time last_sonar_time; |
|
170 |
ros::Duration sonar_tick_time; |
|
171 |
|
|
158 | 172 |
// A vector of the 8 linesensor readings |
159 | 173 |
std::vector<unsigned int> linesensor_readings; |
160 | 174 |
|
... | ... | |
167 | 181 |
ros::Subscriber motors_sub; |
168 | 182 |
ros::Publisher pose_pub; |
169 | 183 |
ros::Publisher color_pub; |
184 |
ros::Publisher sonar_pub; |
|
170 | 185 |
ros::ServiceServer set_pen_srv; |
171 | 186 |
ros::ServiceServer query_encoders_srv; |
172 | 187 |
ros::ServiceServer query_linesensor_srv; |
Also available in: Unified diff