Quadrotor

/**

 * @brief A node responsible for reading joystick messages from ros

 *        joystick_drivers to control the MikroKopter

 *

 * @author CMU Quadrotor Project

 */

#include <ros/ros.h>

#include <sensor_msgs/Joy.h>

#include <stdio.h>

#include "nav_lib.h"

class JoystickControl

{

    public:

        JoystickControl();

        void joyLoop();

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

    private:

        MikrokopterControl control;

        ros::Publisher pub;

        ros::NodeHandle n;

        ros::Subscriber sub;

};

JoystickControl::JoystickControl()

{

  pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100); 

  sub = n.subscribe<sensor_msgs::Joy>("joy", 100,

      &JoystickControl::joyCallback, this);

}

void JoystickControl::joyLoop()

{

    ros::Rate loop_rate(25);

    while(ros::ok())

    {

        ros::spinOnce(); // if joyCallback is called, it will be in here

        control.publish_on(pub);

        loop_rate.sleep();

    }

    return;

}

void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)

{

    control.velocity_control(joy->axes[1], -joy->axes[0]);

    control.set_yaw(-joy->axes[2]);

    control.set_thrust((joy->axes[3]+1)/2); ///Adjust thrust to be between [0, 1] instead of [-1, 1]

    return;

}

int main(int argc, char** argv)

{

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

    JoystickControl joy_control;

    // This commands loops infinitely

    joy_control.joyLoop();

    return 0;

}