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