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