Quadrotor

#include <ros/ros.h>

#include <string.h>

#include <geometry_msgs/Quaternion.h>

#include <interactive_markers/interactive_marker_server.h>

#include <functional>

using namespace std;

using namespace visualization_msgs;

/* TODO:

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

 */

class Marker3DOF

{

public:

  Marker3DOF(interactive_markers::InteractiveMarkerServer& server, int pubRate);

  void processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback);

  void timerCallback(const ros::TimerEvent& event);

private:

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

  ros::NodeHandle nh;

  ros::Publisher pub;

  ros::Timer timer;

  InteractiveMarker marker;

};

Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server,

    int pubRate = 0)

{

  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", bind(&Marker3DOF::processFeedback, this, _1));

  server.applyChanges();

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

  if (pubRate > 0)

    timer = nh.createTimer(ros::Duration(1./pubRate),

        bind(&Marker3DOF::timerCallback, this, _1));

}

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)

{

  marker.pose.position = feedback->pose.position;

}

void Marker3DOF::timerCallback(const ros::TimerEvent& event)

{

  pub.publish(marker.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, 10);

  ros::spin();

  return 0;

}