root / mikrokopter / mikrokopter / src / joystick_control.cpp @ 98711613
History | View | Annotate | Download (1.2 KB)
1 | 5d4b79f4 | Tom Mullins | /**
|
---|---|---|---|
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 | sub = n.subscribe<sensor_msgs::Joy>("joy", 100, |
||
31 | &JoystickControl::joyCallback, this);
|
||
32 | } |
||
33 | |||
34 | void JoystickControl::joyLoop()
|
||
35 | { |
||
36 | 7a6ed02d | Tom Mullins | control.main_loop(); |
37 | 5d4b79f4 | Tom Mullins | } |
38 | |||
39 | void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) |
||
40 | { |
||
41 | fd29b28e | Priya | control.velocity_control(joy->axes[1], -joy->axes[0]); |
42 | control.set_yaw(-joy->axes[2]);
|
||
43 | control.set_thrust((joy->axes[3]+1)/2); ///Adjust thrust to be between [0, 1] instead of [-1, 1] |
||
44 | return;
|
||
45 | 5d4b79f4 | Tom Mullins | } |
46 | |||
47 | int main(int argc, char** argv) |
||
48 | { |
||
49 | ros::init(argc, argv, "joystick_control");
|
||
50 | JoystickControl joy_control; |
||
51 | |||
52 | // This commands loops infinitely
|
||
53 | joy_control.joyLoop(); |
||
54 | |||
55 | return 0; |
||
56 | } |