scoutos / scout / scoutsim / src / scout.h @ a8480867
History | View | Annotate | Download (3.54 KB)
1 |
/*
|
---|---|
2 |
* This package was developed by the CMU Robotics Club project using code
|
3 |
* from Willow Garage, Inc. Please see licensing.txt for details.
|
4 |
*
|
5 |
* All rights reserved.
|
6 |
*
|
7 |
* @file scout.h
|
8 |
* @author Colony Project, CMU Robotics Club
|
9 |
* @author Alex Zirbel
|
10 |
*/
|
11 |
|
12 |
#ifndef SCOUTSIM_SCOUT_H
|
13 |
#define SCOUTSIM_SCOUT_H
|
14 |
|
15 |
#include <ros/ros.h> |
16 |
#include <boost/shared_ptr.hpp> |
17 |
|
18 |
#include <motors/set_motors.h> |
19 |
|
20 |
#include <scoutsim/Pose.h> |
21 |
#include <scoutsim/Velocity.h> |
22 |
#include <scoutsim/SetPen.h> |
23 |
#include <scoutsim/TeleportRelative.h> |
24 |
#include <scoutsim/TeleportAbsolute.h> |
25 |
#include <scoutsim/Color.h> |
26 |
|
27 |
#include <wx/wx.h> |
28 |
|
29 |
#define PI 3.14159265 |
30 |
|
31 |
namespace scoutsim |
32 |
{ |
33 |
struct Vector2
|
34 |
{ |
35 |
Vector2() |
36 |
: x(0.0) |
37 |
, y(0.0) |
38 |
{} |
39 |
|
40 |
Vector2(float _x, float _y) |
41 |
: x(_x) |
42 |
, y(_y) |
43 |
{} |
44 |
|
45 |
bool operator==(const Vector2& rhs) |
46 |
{ |
47 |
return x == rhs.x && y == rhs.y;
|
48 |
} |
49 |
|
50 |
bool operator!=(const Vector2& rhs) |
51 |
{ |
52 |
return x != rhs.x || y != rhs.y;
|
53 |
} |
54 |
|
55 |
float x;
|
56 |
float y;
|
57 |
}; |
58 |
|
59 |
class Scout |
60 |
{ |
61 |
public:
|
62 |
Scout(const ros::NodeHandle& nh, const wxImage& scout_image, |
63 |
const Vector2& pos, float orient); |
64 |
|
65 |
void update(double dt, wxMemoryDC& path_dc, |
66 |
const wxImage& path_image, wxColour background_color,
|
67 |
float canvas_width, float canvas_height); |
68 |
void paint(wxDC& dc);
|
69 |
|
70 |
private:
|
71 |
void setMotors(const motors::set_motors::ConstPtr& msg); |
72 |
void velocityCallback(const VelocityConstPtr& vel); |
73 |
bool setPenCallback(scoutsim::SetPen::Request&,
|
74 |
scoutsim::SetPen::Response&); |
75 |
bool teleportRelativeCallback(scoutsim::TeleportRelative::Request&,
|
76 |
scoutsim::TeleportRelative::Response&); |
77 |
bool teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request&,
|
78 |
scoutsim::TeleportAbsolute::Response&); |
79 |
|
80 |
ros::NodeHandle n_; |
81 |
|
82 |
wxImage scout_image_; |
83 |
wxBitmap scout_; |
84 |
|
85 |
Vector2 pos_; |
86 |
float orient_;
|
87 |
|
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_;
|
93 |
|
94 |
float lin_vel_;
|
95 |
float ang_vel_;
|
96 |
bool pen_on_;
|
97 |
wxPen pen_; |
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_; |
106 |
|
107 |
ros::WallTime last_command_time_; |
108 |
|
109 |
float meter_;
|
110 |
|
111 |
struct TeleportRequest
|
112 |
{ |
113 |
TeleportRequest(float x, float y, float _theta, |
114 |
float _linear, bool _relative) |
115 |
: pos(x, y) |
116 |
, theta(_theta) |
117 |
, linear(_linear) |
118 |
, relative(_relative) |
119 |
{} |
120 |
|
121 |
Vector2 pos; |
122 |
float theta;
|
123 |
float linear;
|
124 |
bool relative;
|
125 |
}; |
126 |
|
127 |
typedef std::vector<TeleportRequest> V_TeleportRequest;
|
128 |
V_TeleportRequest teleport_requests_; |
129 |
}; |
130 |
typedef boost::shared_ptr<Scout> ScoutPtr;
|
131 |
|
132 |
} |
133 |
|
134 |
#endif
|