root / quad2 / joystick_node / src / joystick_node.cpp @ 98e35b23
History | View | Annotate | Download (1.86 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[0]; |
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 |
} |