Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / joystick_control.cpp @ 98711613

History | View | Annotate | Download (1.2 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
  sub = n.subscribe<sensor_msgs::Joy>("joy", 100,
31
      &JoystickControl::joyCallback, this);
32
}
33

    
34
void JoystickControl::joyLoop()
35
{
36
    control.main_loop();
37
}
38

    
39
void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
40
{
41
    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
}
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
}