Statistics
| Branch: | Revision:

root / mikrokopter / joystick_node / src / joystick_node.cpp @ 807b0a81

History | View | Annotate | Download (1.93 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

    
7
// TODO
8
// publish to /altitude_node/enable based on a button
9
// publish velocity like in joystick_control.cpp
10

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

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

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

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

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