Revision a8480867
ID | a84808670408804bf5ce08741a3b59130351928f |
Made a lot of changes to the general structure.
Applied object-orienting techniques to the code, cleaning it up considerably. Major design changes are as follows:
- All separate node code (ie motors) are meant to only interface with hardware - doing as little logic as possible.
- Instead, the logic is moved to a class (ie MotorControl) in libscout. PID and other tools should be located here. This significantly reduces dependencies.
- Files can be included cross-package by being placed in /include/packagename, but this should be avoided to reduce dependencies.
A few formatting changes:
- Comments should end with / always, not */
- Use @ instead of \
- No double newlines.
The motors node is revamped and pretty much correct at this point.
scout/scoutsim/src/scout.cpp | ||
---|---|---|
10 | 10 |
* @author Alex Zirbel |
11 | 11 |
*/ |
12 | 12 |
|
13 |
#include "scoutsim/scout.h"
|
|
13 |
#include "scout.h" |
|
14 | 14 |
|
15 | 15 |
#include <wx/wx.h> |
16 | 16 |
|
... | ... | |
20 | 20 |
|
21 | 21 |
namespace scoutsim |
22 | 22 |
{ |
23 |
|
|
24 | 23 |
Scout::Scout(const ros::NodeHandle& nh, |
25 | 24 |
const wxImage& scout_image, |
26 | 25 |
const Vector2& pos, |
27 | 26 |
float orient) |
28 |
: nh_(nh)
|
|
27 |
: n_(nh) |
|
29 | 28 |
, scout_image_(scout_image) |
30 | 29 |
, pos_(pos) |
31 | 30 |
, orient_(orient) |
... | ... | |
37 | 36 |
pen_.SetWidth(3); |
38 | 37 |
scout_ = wxBitmap(scout_image_); |
39 | 38 |
|
39 |
motors_sub_ = n_.subscribe("set_motors", 1, &Scout::setMotors, this); |
|
40 |
|
|
41 |
/// @TODO take this out! |
|
40 | 42 |
velocity_sub_ = |
41 |
nh_.subscribe("command_velocity",
|
|
43 |
n_.subscribe("command_velocity", |
|
42 | 44 |
1, |
43 | 45 |
&Scout::velocityCallback, this); |
44 |
pose_pub_ = nh_.advertise<Pose>("pose", 1);
|
|
45 |
color_pub_ = nh_.advertise<Color>("color_sensor", 1);
|
|
46 |
set_pen_srv_ = nh_.advertiseService("set_pen",
|
|
46 |
pose_pub_ = n_.advertise<Pose>("pose", 1); |
|
47 |
color_pub_ = n_.advertise<Color>("color_sensor", 1); |
|
48 |
set_pen_srv_ = n_.advertiseService("set_pen", |
|
47 | 49 |
&Scout::setPenCallback, |
48 | 50 |
this); |
49 | 51 |
teleport_relative_srv_ = |
50 |
nh_.advertiseService("teleport_relative",
|
|
52 |
n_.advertiseService("teleport_relative", |
|
51 | 53 |
&Scout::teleportRelativeCallback, |
52 | 54 |
this); |
53 | 55 |
teleport_absolute_srv_ = |
54 |
nh_.advertiseService("teleport_absolute",
|
|
56 |
n_.advertiseService("teleport_absolute", |
|
55 | 57 |
&Scout::teleportAbsoluteCallback, |
56 | 58 |
this); |
57 | 59 |
|
58 | 60 |
meter_ = scout_.GetHeight(); |
59 | 61 |
} |
60 | 62 |
|
63 |
/** |
|
64 |
* A callback function that sets velocity based on a set_motors |
|
65 |
* request. |
|
66 |
*/ |
|
67 |
void Scout::setMotors(const motors::set_motors::ConstPtr& msg) |
|
68 |
{ |
|
69 |
last_command_time_ = ros::WallTime::now(); |
|
70 |
|
|
71 |
if(msg->fl_set) |
|
72 |
{ |
|
73 |
motor_fl_speed_ = msg->fl_speed; |
|
74 |
} |
|
75 |
if(msg->fr_set) |
|
76 |
{ |
|
77 |
motor_fr_speed_ = msg->fr_speed; |
|
78 |
} |
|
79 |
if(msg->bl_set) |
|
80 |
{ |
|
81 |
motor_bl_speed_ = msg->bl_speed; |
|
82 |
} |
|
83 |
if(msg->br_set) |
|
84 |
{ |
|
85 |
motor_br_speed_ = msg->br_speed; |
|
86 |
} |
|
87 |
|
|
88 |
float l_speed = (float (motor_fl_speed_ + motor_bl_speed_)) / 2; |
|
89 |
float r_speed = (float (motor_fr_speed_ + motor_br_speed_)) / 2; |
|
90 |
|
|
91 |
/// @TODO Change this! |
|
92 |
lin_vel_ = (l_speed + r_speed) / 2; |
|
93 |
ang_vel_ = r_speed - l_speed; |
|
94 |
|
|
95 |
} |
|
61 | 96 |
|
62 | 97 |
void Scout::velocityCallback(const VelocityConstPtr& vel) |
63 | 98 |
{ |
... | ... | |
206 | 241 |
} |
207 | 242 |
|
208 | 243 |
ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", |
209 |
nh_.getNamespace().c_str(), pos_.x, pos_.y, orient_);
|
|
244 |
n_.getNamespace().c_str(), pos_.x, pos_.y, orient_); |
|
210 | 245 |
|
211 | 246 |
if (pen_on_) |
212 | 247 |
{ |
Also available in: Unified diff