Quadrotor

#include <ros/ros.h>

#include <string.h>

#include <geometry_msgs/Quaternion.h>

#include <interactive_markers/interactive_marker_server.h>

using namespace std;

using namespace visualization_msgs;

/* TODO:

 * -replace the three MOVE_AXIS directions with one MOVE_AXIS and one MOVE_PLANE

 * -make it compile without needing class Callback

 * -have it constantly publish current position, instead of only publishing

 *  when the marker is moved

 */

class Marker3DOF

{

public:

  Marker3DOF(interactive_markers::InteractiveMarkerServer& server);

  void processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback);

private:

  void addControl(string name, float x, float y, float z, float w);

  ros::NodeHandle nh;

  InteractiveMarker marker;

  ros::Publisher pub;

};

// boost should be able to do this, but... I could _not_ make it compile

// so, I wrote my own

template <class class_t, class arg_t> class Callback

{

public:

  Callback(void (class_t::*cb)(arg_t), class_t* obj) : cb(cb), obj(obj) {}

  void operator()(arg_t arg) {(obj->*cb)(arg);}

private:

  void (class_t::*cb)(arg_t);

  class_t* obj;

};

Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server)

{

  marker.header.frame_id = "/quadrotor";

  marker.pose.position.x = 5;

  marker.pose.position.y = 0;

  marker.pose.position.z = 0;

  marker.scale = 1;

  marker.name = "target";

  addControl("target_x", 1, 0, 0, 1);

  addControl("target_y", 0, 1, 0, 1);

  addControl("target_z", 0, 0, 1, 1);

  server.insert(marker);

  server.setCallback("target", Callback<Marker3DOF, const

      InteractiveMarkerFeedbackConstPtr&>(&Marker3DOF::processFeedback, this));

  server.applyChanges();

  pub = nh.advertise<geometry_msgs::Point>("/v2v3_converter/target_3d", 100);

}

void Marker3DOF::addControl(string name, float x, float y, float z, float w)

{

  InteractiveMarkerControl control;

  control.orientation.x = x;

  control.orientation.y = y;

  control.orientation.z = z;

  control.orientation.w = w;

  control.name = name;

  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;

  marker.controls.push_back(control);

}

void Marker3DOF::processFeedback(const InteractiveMarkerFeedbackConstPtr&

    feedback)

{

  pub.publish(feedback->pose.position);

}

int main(int argc, char **argv)

{

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

  interactive_markers::InteractiveMarkerServer server(

      "rviz_interactive_targets");

  Marker3DOF marker(server);

  ros::spin();

  return 0;

}