Statistics
| Branch: | Revision:

root / quad2 / joystick_node / src / joystick_node.cpp @ 122a57d8

History | View | Annotate | Download (1.88 KB)

1
#include "ros/ros.h"
2
#include "std_msgs/Float64.h"
3
#include "std_msgs/Bool.h"
4
#include "sensor_msgs/Joy.h"
5
#include "mikrokopter/Velocity2D.h"
6
#include <iostream>
7

    
8
using namespace std;
9

    
10
class JoystickNode
11
{
12
public:
13
  JoystickNode();
14
  void main_loop() {ros::spin();}
15
  void joy_cb(const sensor_msgs::Joy::ConstPtr& joy);
16

    
17
private:
18
  bool altitude_enabled;
19
  bool alt_en_pressed;
20
  ros::NodeHandle nh;
21
  ros::Subscriber joy_sub;
22
  ros::Publisher velocity_pub;
23
  ros::Publisher yaw_pub;
24
  ros::Publisher thrust_pub;
25
  ros::Publisher altitude_enable_pub;
26
};
27

    
28
JoystickNode::JoystickNode() : altitude_enabled(false), alt_en_pressed(false)
29
{
30
  joy_sub = nh.subscribe<sensor_msgs::Joy>("/joy", 100, &JoystickNode::joy_cb,
31
      this);
32
  velocity_pub = nh.advertise<mikrokopter::Velocity2D>("/mikrokopter/velocity",
33
      100);
34
  yaw_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/yaw", 100);
35
  thrust_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
36
  altitude_enable_pub = nh.advertise<std_msgs::Bool>("/altitude_node/enable",
37
      100);
38
}
39

    
40
void JoystickNode::joy_cb(const sensor_msgs::Joy::ConstPtr& joy)
41
{
42
  mikrokopter::Velocity2D vel;
43
  vel.forward = joy->axes[1];
44
  vel.lateral = -joy->axes[0];
45
  velocity_pub.publish(vel);
46
  
47
  std_msgs::Float64 yaw;
48
  yaw.data = -joy->axes[2];
49
  yaw_pub.publish(yaw);
50
  
51
  bool alt_en_down = joy->buttons[10]; // button 11
52
  if (alt_en_down && !alt_en_pressed) // toggle altitude control
53
  {
54
    altitude_enabled = !altitude_enabled;
55
    
56
    std_msgs::Bool enable;
57
    enable.data = altitude_enabled;
58
    altitude_enable_pub.publish(enable);
59
  }
60
  alt_en_pressed = alt_en_down;
61
  
62
  if (!altitude_enabled)
63
  {
64
    std_msgs::Float64 thrust;
65
    thrust.data = (joy->axes[3]+1)/2;
66
    thrust_pub.publish(thrust);
67
  }
68
}
69

    
70
int main(int argc, char **argv) {
71
  ros::init(argc, argv, "joystick_node");
72
  JoystickNode jn = JoystickNode();
73
  jn.main_loop();
74
  return 0;
75
}