Go to the documentation of this file.00001 #include <stdio.h>
00002 #include <vector>
00003 #include <math.h>
00004 #include <visualization_msgs/Marker.h>
00005 #include <visualization_msgs/MarkerArray.h>
00006 #include <numeric>
00007 #include <interactive_markers/interactive_marker_server.h>
00008 #include <interactive_markers/menu_handler.h>
00009 #include <boost/lexical_cast.hpp>
00010 #include <boost/format.hpp>
00011 #include "std_msgs/String.h"
00012 #include <trajectory_planner/coordinates.h>
00013
00014
00015 using namespace visualization_msgs;
00016
00017
00018 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00019 ros::NodeHandle* p_n;
00020 ros::Publisher coor_pub;
00021 trajectory_planner::coordinates message;
00022
00023
00024
00025 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00026 {
00027 if( feedback->mouse_point_valid )
00028 {
00029
00030
00031
00032
00033 }
00034
00035 switch ( feedback->event_type )
00036 {
00037 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00038 message.x=feedback->pose.position.x; message.y=feedback->pose.position.y; message.theta=2.0*asin(feedback->pose.orientation.z);
00039
00040 message.header.stamp = ros::Time::now();
00041 message.header.frame_id = "/world";
00042
00043 coor_pub.publish(message);
00044
00045 break;
00046 }
00047
00048 server->applyChanges();
00049 }
00050
00051
00052 Marker makeBox( InteractiveMarker &msg )
00053 {
00054 Marker marker;
00055
00056 marker.type = Marker::ARROW;
00057 marker.scale.x = msg.scale;
00058 marker.scale.y = msg.scale;
00059 marker.scale.z = msg.scale;
00060 marker.color.r = 0;
00061 marker.color.g = 0.7;
00062 marker.color.b = 0;
00063 marker.color.a = 0.6;
00064 return marker;
00065 }
00066
00067
00068 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00069 {
00070 InteractiveMarkerControl control;
00071 control.always_visible = true;
00072 control.markers.push_back( makeBox(msg) );
00073 msg.controls.push_back( control );
00074
00075 return msg.controls.back();
00076 }
00077
00078
00079 void make6DofMarker( bool fixed )
00080 {
00081 InteractiveMarker int_marker;
00082 int_marker.header.frame_id = "/world";
00083 int_marker.pose.position.x = 0;
00084 int_marker.pose.position.y = 0;
00085 int_marker.pose.position.z = 0;
00086 int_marker.scale = 0.4;
00087
00088 int_marker.name = "Control target";
00089 int_marker.description = "Control the final \nposition of the robot";
00090
00091
00092 makeBoxControl(int_marker);
00093 InteractiveMarkerControl control;
00094
00095 control.orientation.w = 1;
00096 control.orientation.x = 1;
00097 control.orientation.y = 0;
00098 control.orientation.z = 0;
00099 control.name = "move_x";
00100 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00101 int_marker.controls.push_back(control);
00102
00103 control.orientation.w = 1;
00104 control.orientation.x = 0;
00105 control.orientation.y = 0;
00106 control.orientation.z = 1;
00107 control.name = "move_y";
00108 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00109 int_marker.controls.push_back(control);
00110
00111 control.orientation.w = 1;
00112 control.orientation.x = 0;
00113 control.orientation.y = 1;
00114 control.orientation.z = 0;
00115 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00116 control.name = "rotate_z";
00117 int_marker.controls.push_back(control);
00118
00119 server->insert(int_marker);
00120 server->setCallback(int_marker.name, &processFeedback);
00121 }
00122
00123
00124 int main(int argc, char **argv)
00125 {
00126 ros::init(argc, argv, "APgenerator");
00127 ros::NodeHandle n("~");
00128 ros::Rate loop_rate(10);
00129 p_n=&n;
00130
00131 coor_pub = n.advertise<trajectory_planner::coordinates>("/msg_coordinates", 1000);
00132
00133
00134 server.reset( new interactive_markers::InteractiveMarkerServer("APgenerator/im","",false) );
00135 make6DofMarker( true );
00136 server->applyChanges();
00137
00138 while(ros::ok())
00139 {
00140
00141
00142
00143 loop_rate.sleep();
00144 ros::spinOnce();
00145 }
00146
00147 server.reset();
00148 }