Quadrotor

#include "ros/ros.h"

#include "std_msgs/Float64.h"

#include "std_msgs/Bool.h"

#include "sensor_msgs/Joy.h"

#include "mikrokopter/Velocity2D.h"

// TODO

// publish to /altitude_node/enable based on a button

// publish velocity like in joystick_control.cpp

class JoystickNode

{

public:

  JoystickNode();

  void main_loop() {ros::spin();}

  void joy_cb(const sensor_msgs::Joy::ConstPtr& joy);

private:

  bool altitude_enabled;

  bool alt_en_pressed;

  ros::NodeHandle nh;

  ros::Subscriber joy_sub;

  ros::Publisher velocity_pub;

  ros::Publisher yaw_pub;

  ros::Publisher thrust_pub;

  ros::Publisher altitude_enable_pub;

};

JoystickNode::JoystickNode() : altitude_enabled(false), alt_en_pressed(false)

{

  joy_sub = nh.subscribe<sensor_msgs::Joy>("/joy", 100, &JoystickNode::joy_cb,

      this);

  velocity_pub = nh.advertise<mikrokopter::Velocity2D>("/mikrokopter/velocity",

      100);

  yaw_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/yaw", 100);

  thrust_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);

  altitude_enable_pub = nh.advertise<std_msgs::Bool>("/altitude_node/enable",

      100);

}

void JoystickNode::joy_cb(const sensor_msgs::Joy::ConstPtr& joy)

{

  mikrokopter::Velocity2D vel;

  vel.forward = joy->axes[1];

  vel.lateral = -joy->axes[0];

  velocity_pub.publish(vel);

  std_msgs::Float64 yaw;

  yaw.data = -joy->axes[2];

  yaw_pub.publish(yaw);

  bool alt_en_down = joy->buttons[0];

  if (alt_en_down && !alt_en_pressed) // toggle altitude control

  {

    altitude_enabled = !altitude_enabled;

    std_msgs::Bool enable;

    enable.data = altitude_enabled;

    altitude_enable_pub.publish(enable);

  }

  alt_en_pressed = alt_en_down;

  if (!altitude_enabled)

  {

    std_msgs::Float64 thrust;

    thrust.data = (joy->axes[3]+1)/2;

    thrust_pub.publish(thrust);

  }

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "joystick_node");

  JoystickNode jn = JoystickNode();

  jn.main_loop();

  return 0;

}