root / quad2 / joystick_node / src / joystick_node.cpp @ 122a57d8
History | View | Annotate | Download (1.88 KB)
1 | 807b0a81 | Tom Mullins | #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 | 98e35b23 | Alex Zirbel | #include <iostream> |
7 | 807b0a81 | Tom Mullins | |
8 | 98e35b23 | Alex Zirbel | using namespace std; |
9 | 807b0a81 | Tom Mullins | |
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 | 122a57d8 | Tom Mullins | bool alt_en_down = joy->buttons[10]; // button 11 |
52 | 807b0a81 | Tom Mullins | 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 | } |