Project

General

Profile

Revision 807b0a81

ID807b0a818b6bb6c36e3592074181eb95f17e4ac7

Added by Thomas Mullins almost 12 years ago

Added joystick_node for the new node_control system

It can also enable/disable altitude_node's thrust messages, and publish
its own instead.

View differences:

mikrokopter/joystick_node/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
rosbuild_init()
5

  
6
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
7
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
8

  
9
#rosbuild_genmsg()
10
#rosbuild_gensrv()
11

  
12
rosbuild_add_executable(${PROJECT_NAME} src/joystick_node.cpp)
mikrokopter/joystick_node/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/joystick_node/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b joystick_node is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/joystick_node/manifest.xml
1
<package>
2

  
3
  <description brief="joystick_node">
4
    Publishes roll/pitch/yaw settings to /mikrokopter/velocity and /mikrokopter/yaw
5
  </description>
6
  <author>Tom Mullins</author>
7
  
8
  <depend package="roscpp"/>
9
  <depend package="std_msgs"/>
10
  <depend package="sensor_msgs"/>
11
  <depend package="mikrokopter"/>
12

  
13
</package>
14

  
15

  
mikrokopter/joystick_node/src/joystick_node.cpp
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
}

Also available in: Unified diff