root / mikrokopter / src / joystick_control.cpp @ 5d4b79f4
History | View | Annotate | Download (1.32 KB)
1 |
/**
|
---|---|
2 |
* @brief A node responsible for reading joystick messages from ros
|
3 |
* joystick_drivers to control the MikroKopter
|
4 |
*
|
5 |
* @author CMU Quadrotor Project
|
6 |
*/
|
7 |
|
8 |
#include <ros/ros.h> |
9 |
#include <sensor_msgs/Joy.h> |
10 |
#include <stdio.h> |
11 |
|
12 |
#include "nav_lib.h" |
13 |
|
14 |
class JoystickControl |
15 |
{ |
16 |
public:
|
17 |
JoystickControl(); |
18 |
void joyLoop();
|
19 |
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); |
20 |
|
21 |
private:
|
22 |
MikrokopterControl control; |
23 |
ros::Publisher pub; |
24 |
ros::NodeHandle n; |
25 |
ros::Subscriber sub; |
26 |
}; |
27 |
|
28 |
JoystickControl::JoystickControl() |
29 |
{ |
30 |
pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100); |
31 |
sub = n.subscribe<sensor_msgs::Joy>("joy", 100, |
32 |
&JoystickControl::joyCallback, this);
|
33 |
} |
34 |
|
35 |
void JoystickControl::joyLoop()
|
36 |
{ |
37 |
ros::Rate loop_rate(25);
|
38 |
while(ros::ok())
|
39 |
{ |
40 |
ros::spinOnce(); // if joyCallback is called, it will be in here
|
41 |
control.publish_on(pub); |
42 |
loop_rate.sleep(); |
43 |
} |
44 |
return;
|
45 |
} |
46 |
|
47 |
void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) |
48 |
{ |
49 |
// TODO call control.velocity_control(...) based on Joy message
|
50 |
} |
51 |
|
52 |
int main(int argc, char** argv) |
53 |
{ |
54 |
ros::init(argc, argv, "joystick_control");
|
55 |
JoystickControl joy_control; |
56 |
|
57 |
// This commands loops infinitely
|
58 |
joy_control.joyLoop(); |
59 |
|
60 |
return 0; |
61 |
} |