root / mikrokopter / joystick_node / src / joystick_node.cpp @ 807b0a81
History | View | Annotate | Download (1.93 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 | |||
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 | } |