Revision 144137a1
ID | 144137a12c1437f71d49b144279823dc836cb14b |
Got motor control working with scoutsim
Behaviors can now use the MotorControl class to change the speed of the motors in the simulator. The simulator correctly handles the command.
At the moment, the set_motors command is limited to 'scout1'. We should look into prefixes to specify which scout (scout1, scout2, etc) each behavior should command.
Also changed alex_behavior to be a good demonstration of the motors.
scout/scoutsim/src/scout.h | ||
---|---|---|
9 | 9 |
* @author Alex Zirbel |
10 | 10 |
*/ |
11 | 11 |
|
12 |
#ifndef SCOUTSIM_SCOUT_H
|
|
13 |
#define SCOUTSIM_SCOUT_H
|
|
12 |
#ifndef _SCOUTSIM_SCOUT_H_
|
|
13 |
#define _SCOUTSIM_SCOUT_H_
|
|
14 | 14 |
|
15 | 15 |
#include <ros/ros.h> |
16 | 16 |
#include <boost/shared_ptr.hpp> |
... | ... | |
18 | 18 |
#include <motors/set_motors.h> |
19 | 19 |
|
20 | 20 |
#include <scoutsim/Pose.h> |
21 |
#include <scoutsim/Velocity.h> |
|
22 | 21 |
#include <scoutsim/SetPen.h> |
23 | 22 |
#include <scoutsim/TeleportRelative.h> |
24 | 23 |
#include <scoutsim/TeleportAbsolute.h> |
... | ... | |
27 | 26 |
#include <wx/wx.h> |
28 | 27 |
|
29 | 28 |
#define PI 3.14159265 |
29 |
/// The scale factor so the speed of scout robots is reasonable for the sim |
|
30 |
#define SPEED_SCALE_FACTOR 0.05 |
|
30 | 31 |
|
31 | 32 |
namespace scoutsim |
32 | 33 |
{ |
... | ... | |
37 | 38 |
, y(0.0) |
38 | 39 |
{} |
39 | 40 |
|
40 |
Vector2(float _x, float _y)
|
|
41 |
: x(_x) |
|
42 |
, y(_y) |
|
41 |
Vector2(float new_x, float new_y)
|
|
42 |
: x(new_x)
|
|
43 |
, y(new_y)
|
|
43 | 44 |
{} |
44 | 45 |
|
45 | 46 |
bool operator==(const Vector2& rhs) |
... | ... | |
69 | 70 |
|
70 | 71 |
private: |
71 | 72 |
void setMotors(const motors::set_motors::ConstPtr& msg); |
72 |
void velocityCallback(const VelocityConstPtr& vel); |
|
73 | 73 |
bool setPenCallback(scoutsim::SetPen::Request&, |
74 | 74 |
scoutsim::SetPen::Response&); |
75 | 75 |
bool teleportRelativeCallback(scoutsim::TeleportRelative::Request&, |
... | ... | |
77 | 77 |
bool teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request&, |
78 | 78 |
scoutsim::TeleportAbsolute::Response&); |
79 | 79 |
|
80 |
ros::NodeHandle n_;
|
|
80 |
ros::NodeHandle node;
|
|
81 | 81 |
|
82 |
wxImage scout_image_;
|
|
83 |
wxBitmap scout_;
|
|
82 |
wxImage scout_image; |
|
83 |
wxBitmap scout; |
|
84 | 84 |
|
85 |
Vector2 pos_;
|
|
86 |
float orient_;
|
|
85 |
Vector2 pos; |
|
86 |
float orient; |
|
87 | 87 |
|
88 | 88 |
// Keep track of the last commanded speeds sent to the sim |
89 |
int motor_fl_speed_;
|
|
90 |
int motor_fr_speed_;
|
|
91 |
int motor_bl_speed_;
|
|
92 |
int motor_br_speed_;
|
|
89 |
int motor_fl_speed; |
|
90 |
int motor_fr_speed; |
|
91 |
int motor_bl_speed; |
|
92 |
int motor_br_speed; |
|
93 | 93 |
|
94 |
float lin_vel_;
|
|
95 |
float ang_vel_;
|
|
96 |
bool pen_on_;
|
|
97 |
wxPen pen_;
|
|
94 |
float lin_vel; |
|
95 |
float ang_vel; |
|
96 |
bool pen_on; |
|
97 |
wxPen pen; |
|
98 | 98 |
|
99 |
ros::Subscriber motors_sub_; |
|
100 |
ros::Subscriber velocity_sub_; |
|
101 |
ros::Publisher pose_pub_; |
|
102 |
ros::Publisher color_pub_; |
|
103 |
ros::ServiceServer set_pen_srv_; |
|
104 |
ros::ServiceServer teleport_relative_srv_; |
|
105 |
ros::ServiceServer teleport_absolute_srv_; |
|
99 |
ros::Subscriber motors_sub; |
|
100 |
ros::Publisher pose_pub; |
|
101 |
ros::Publisher color_pub; |
|
102 |
ros::ServiceServer set_pen_srv; |
|
103 |
ros::ServiceServer teleport_relative_srv; |
|
104 |
ros::ServiceServer teleport_absolute_srv; |
|
106 | 105 |
|
107 |
ros::WallTime last_command_time_;
|
|
106 |
ros::WallTime last_command_time; |
|
108 | 107 |
|
109 |
float meter_;
|
|
108 |
float meter; |
|
110 | 109 |
|
111 | 110 |
struct TeleportRequest |
112 | 111 |
{ |
113 |
TeleportRequest(float x, float y, float _theta, |
|
114 |
float _linear, bool _relative)
|
|
112 |
TeleportRequest(float x, float y, float new_theta,
|
|
113 |
float new_linear, bool new_relative)
|
|
115 | 114 |
: pos(x, y) |
116 |
, theta(_theta) |
|
117 |
, linear(_linear) |
|
118 |
, relative(_relative) |
|
115 |
, theta(new_theta)
|
|
116 |
, linear(new_linear)
|
|
117 |
, relative(new_relative)
|
|
119 | 118 |
{} |
120 | 119 |
|
121 | 120 |
Vector2 pos; |
... | ... | |
125 | 124 |
}; |
126 | 125 |
|
127 | 126 |
typedef std::vector<TeleportRequest> V_TeleportRequest; |
128 |
V_TeleportRequest teleport_requests_;
|
|
127 |
V_TeleportRequest teleport_requests; |
|
129 | 128 |
}; |
130 | 129 |
typedef boost::shared_ptr<Scout> ScoutPtr; |
131 | 130 |
|
Also available in: Unified diff