00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <ros/ros.h>
00033
00034 #include <interactive_markers/interactive_marker_server.h>
00035
00036
00037 interactive_markers::InteractiveMarkerServer* server;
00038
00039 visualization_msgs::InteractiveMarker makeMarker( float r, float g, float b )
00040 {
00041
00042 visualization_msgs::InteractiveMarker int_marker;
00043 int_marker.header.frame_id = "/base_link";
00044 int_marker.name = "my_marker";
00045 int_marker.description = "Simple 1-DOF Control";
00046
00047
00048 visualization_msgs::Marker box_marker;
00049 box_marker.type = visualization_msgs::Marker::CUBE;
00050 box_marker.scale.x = 0.45;
00051 box_marker.scale.y = 0.45;
00052 box_marker.scale.z = 0.45;
00053 box_marker.color.r = r;
00054 box_marker.color.g = g;
00055 box_marker.color.b = b;
00056 box_marker.color.a = 1.0;
00057
00058
00059 visualization_msgs::InteractiveMarkerControl box_control;
00060 box_control.always_visible = true;
00061 box_control.markers.push_back( box_marker );
00062
00063
00064 int_marker.controls.push_back( box_control );
00065
00066
00067
00068
00069 visualization_msgs::InteractiveMarkerControl linear_control;
00070 linear_control.name = "move_x";
00071 linear_control.interaction_mode =
00072 visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00073
00074
00075 int_marker.controls.push_back(linear_control);
00076
00077 return int_marker;
00078 }
00079
00080 bool is_red = false;
00081
00082 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00083 {
00084 ROS_INFO_STREAM( feedback->marker_name << " is now at "
00085 << feedback->pose.position.x << ", " << feedback->pose.position.y
00086 << ", " << feedback->pose.position.z );
00087
00088 bool changed = false;
00089 visualization_msgs::InteractiveMarker int_marker;
00090
00091
00092
00093 if( feedback->pose.position.x < 0 && !is_red )
00094 {
00095 printf( "turning red.\n" );
00096 is_red = true;
00097 int_marker = makeMarker( 1, 0, 0 );
00098 changed = true;
00099 }
00100 if( feedback->pose.position.x >= 0 && is_red )
00101 {
00102 printf( "turning green.\n" );
00103 is_red = false;
00104 int_marker = makeMarker( 0, 1, 0 );
00105 changed = true;
00106 }
00107
00108 if( changed )
00109 {
00110 printf( "changed.\n" );
00111 int_marker.pose = feedback->pose;
00112 server->insert( int_marker );
00113 server->applyChanges();
00114 }
00115 }
00116
00117 visualization_msgs::InteractiveMarker makeCrazyMarker( bool linear )
00118 {
00119
00120 visualization_msgs::InteractiveMarker int_marker;
00121 int_marker.header.frame_id = "/base_link";
00122 int_marker.name = "crazy_marker";
00123 int_marker.description = "Unusual 1-DOF Control";
00124
00125
00126 visualization_msgs::Marker box_marker;
00127 box_marker.type = visualization_msgs::Marker::CUBE;
00128 box_marker.scale.x = 1;
00129 box_marker.scale.y = .3;
00130 box_marker.scale.z = .3;
00131 box_marker.color.r = .3;
00132 box_marker.color.g = .1;
00133 box_marker.color.b = 1;
00134 box_marker.color.a = 1.0;
00135 box_marker.pose.position.y = -1.0;
00136
00137
00138 visualization_msgs::InteractiveMarkerControl box_control;
00139 box_control.always_visible = true;
00140 box_control.markers.push_back( box_marker );
00141 box_control.name = "crazy";
00142 if( linear )
00143 {
00144 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00145 }
00146 else
00147 {
00148 box_control.orientation.w = 1;
00149 box_control.orientation.x = 0;
00150 box_control.orientation.y = 1;
00151 box_control.orientation.z = 0;
00152 box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00153 }
00154
00155
00156 int_marker.controls.push_back( box_control );
00157 int_marker.pose.position.y = 3;
00158
00159 return int_marker;
00160 }
00161
00162 bool is_linear = true;
00163
00164 void processCrazyFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00165 {
00166 ROS_INFO_STREAM( feedback->marker_name << " is now at pos "
00167 << feedback->pose.position.x << ", "
00168 << feedback->pose.position.y << ", "
00169 << feedback->pose.position.z << "; quat "
00170 << feedback->pose.orientation.x << ", "
00171 << feedback->pose.orientation.y << ", "
00172 << feedback->pose.orientation.z << ", "
00173 << feedback->pose.orientation.w );
00174
00175 bool changed = false;
00176 visualization_msgs::InteractiveMarker int_marker;
00177
00178
00179
00180 if( feedback->pose.orientation.z < 0 && !is_linear )
00181 {
00182 printf( "turning linear.\n" );
00183 is_linear = true;
00184 int_marker = makeCrazyMarker( true );
00185 changed = true;
00186 }
00187 if( feedback->pose.position.x > 0 && is_linear )
00188 {
00189 printf( "turning rotary.\n" );
00190 is_linear = false;
00191 int_marker = makeCrazyMarker( false );
00192 changed = true;
00193 }
00194
00195 if( changed )
00196 {
00197 printf( "changed.\n" );
00198 int_marker.pose.position.x = 0;
00199 int_marker.pose.orientation.x = 0;
00200 int_marker.pose.orientation.y = 0;
00201 int_marker.pose.orientation.z = 0;
00202 int_marker.pose.orientation.w = 1;
00203 server->insert( int_marker );
00204 server->applyChanges();
00205 }
00206 }
00207
00208 int main(int argc, char** argv)
00209 {
00210 ros::init(argc, argv, "interactive_marker_test");
00211
00212
00213 server = new interactive_markers::InteractiveMarkerServer("simple_marker");
00214
00215
00216 visualization_msgs::InteractiveMarker int_marker = makeMarker(0, 1, 0);
00217
00218
00219
00220 server->insert(int_marker, &processFeedback);
00221
00222
00223 visualization_msgs::InteractiveMarker crazy_marker = makeCrazyMarker( true );
00224
00225
00226
00227 server->insert(crazy_marker, &processCrazyFeedback);
00228
00229
00230 server->applyChanges();
00231
00232
00233 ros::spin();
00234 }
00235