Go to the documentation of this file.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
00035 #include <ros/ros.h>
00036 #include <interactive_markers/interactive_marker_server.h>
00037 #include <interactive_markers/menu_handler.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/tf.h>
00040 #include <math.h>
00041 #include <sensor_msgs/JointState.h>
00042
00043 using namespace visualization_msgs;
00044
00045
00046 bool pos_control=true;
00047
00048
00049
00050 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00051 float marker_pos = 0;
00052 interactive_markers::MenuHandler menu_handler;
00053 ros::Publisher pub;
00054
00055
00056
00057 void frameCallback(const ros::TimerEvent&)
00058 {
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 }
00077
00078
00079
00080 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00081 {
00082 std::ostringstream s;
00083 s << "Feedback from marker '" << feedback->marker_name << "' "
00084 << " / control '" << feedback->control_name << "'";
00085
00086 std::ostringstream mouse_point_ss;
00087 if( feedback->mouse_point_valid )
00088 {
00089 mouse_point_ss << " at " << feedback->mouse_point.x
00090 << ", " << feedback->mouse_point.y
00091 << ", " << feedback->mouse_point.z
00092 << " in frame " << feedback->header.frame_id;
00093 }
00094
00095 switch ( feedback->event_type )
00096 {
00097
00098
00099
00100
00101 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00102 ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
00103 if (feedback->menu_entry_id==1)
00104 {
00105 pos_control=true;
00106 ROS_INFO("Selected pos control");
00107 sensor_msgs::JointState joint_state;
00108 joint_state.header.stamp = ros::Time::now();
00109 joint_state.name.resize(1);
00110 joint_state.position.resize(1);
00111 joint_state.velocity.resize(0);
00112 joint_state.name[0] =ros::names::remap("shaft");
00113 joint_state.position[0] = 0;
00114 pub.publish(joint_state);
00115
00116
00117 }
00118 else if (feedback->menu_entry_id==3)
00119 {
00120
00121 pos_control=false;
00122 ROS_INFO("Selected speed control");
00123 sensor_msgs::JointState joint_state;
00124 joint_state.header.stamp = ros::Time::now();
00125 joint_state.name.resize(1);
00126 joint_state.position.resize(1);
00127 joint_state.velocity.resize(1);
00128 joint_state.name[0] =ros::names::remap("shaft");
00129 joint_state.position[0] = 0;
00130 joint_state.velocity[0] = 30;
00131 pub.publish(joint_state);
00132 }
00133 else if (feedback->menu_entry_id==4)
00134 {
00135
00136 pos_control=false;
00137 ROS_INFO("Selected speed control");
00138 sensor_msgs::JointState joint_state;
00139 joint_state.header.stamp = ros::Time::now();
00140 joint_state.name.resize(1);
00141 joint_state.position.resize(1);
00142 joint_state.velocity.resize(1);
00143 joint_state.name[0] =ros::names::remap("shaft");
00144 joint_state.position[0] = 0;
00145 joint_state.velocity[0] = 60;
00146 pub.publish(joint_state);
00147
00148 }
00149
00150 break;
00151
00152 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00153
00154 #if ROS_VERSION_MINIMUM(1, 8, 0)
00155 tf::Quaternion q;
00156 tf::Matrix3x3 M(q);
00157 #else
00158 btQuaternion q;
00159 btMatrix3x3 M(q);
00160 #endif
00161
00162
00163 q[0] = feedback->pose.orientation.x;
00164 q[1] = feedback->pose.orientation.y;
00165 q[2] = feedback->pose.orientation.z;
00166 q[3] = feedback->pose.orientation.w;
00167
00168
00169
00170 double r,p,y;
00171 M.getRPY(r,p,y);
00172 if (pos_control==true)
00173 {
00174 sensor_msgs::JointState joint_state;
00175 joint_state.header.stamp = ros::Time::now();
00176 joint_state.name.resize(1);
00177 joint_state.position.resize(1);
00178 joint_state.velocity.resize(0);
00179 joint_state.name[0] =ros::names::remap("shaft");
00180 joint_state.position[0] = y;
00181 pub.publish(joint_state);
00182 ROS_INFO("new pos %f",y);
00183 }
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208 break;
00209
00210
00211
00212
00213
00214
00215
00216
00217 }
00218
00219 server->applyChanges();
00220 }
00221
00222
00223
00224 void alignMarker( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00225 {
00226 geometry_msgs::Pose pose = feedback->pose;
00227
00228 pose.position.x = round(pose.position.x-0.5)+0.5;
00229 pose.position.y = round(pose.position.y-0.5)+0.5;
00230
00231 ROS_INFO_STREAM( feedback->marker_name << ":"
00232 << " aligning position = "
00233 << feedback->pose.position.x
00234 << ", " << feedback->pose.position.y
00235 << ", " << feedback->pose.position.z
00236 << " to "
00237 << pose.position.x
00238 << ", " << pose.position.y
00239 << ", " << pose.position.z );
00240
00241 server->setPose( feedback->marker_name, pose );
00242 server->applyChanges();
00243 }
00244
00245
00246 double rand( double min, double max )
00247 {
00248 double t = (double)rand() / (double)RAND_MAX;
00249 return min + t*(max-min);
00250 }
00251
00252
00253 Marker makeBox( InteractiveMarker &msg )
00254 {
00255 Marker marker;
00256
00257 marker.type = Marker::SPHERE;
00258 marker.scale.x = msg.scale * 0.45;
00259 marker.scale.y = msg.scale * 0.45;
00260 marker.scale.z = msg.scale * 0.45;
00261 marker.color.r = 0;
00262 marker.color.g = 0.7;
00263 marker.color.b = 0;
00264 marker.color.a = 0.6;
00265
00266 return marker;
00267 }
00268
00269 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00270 {
00271 InteractiveMarkerControl control;
00272 control.always_visible = true;
00273 control.markers.push_back( makeBox(msg) );
00274 msg.controls.push_back( control );
00275
00276 return msg.controls.back();
00277 }
00278
00279
00280 void saveMarker( InteractiveMarker int_marker )
00281 {
00282 server->insert(int_marker);
00283 server->setCallback(int_marker.name, &processFeedback);
00284 }
00285
00287
00288
00289 void make6DofMarker( bool fixed )
00290 {
00291 InteractiveMarker int_marker;
00292 int_marker.header.frame_id = "/atc/vehicle/center_bumper";
00293 int_marker.pose.position.x = 2;
00294 int_marker.pose.position.y = 0;
00295 int_marker.pose.position.z = 0.5;
00296 int_marker.scale = 0.5;
00297
00298 int_marker.name = "Fovetation Control Target";
00299 int_marker.description = "Foveation_control will try \nto aim the PTU towards\n the marker";
00300
00301
00302 makeBoxControl(int_marker);
00303 InteractiveMarkerControl control;
00304
00305 if ( fixed )
00306 {
00307
00308
00309
00310 }
00311
00312 control.orientation.w = 1;
00313 control.orientation.x = 1;
00314 control.orientation.y = 0;
00315 control.orientation.z = 0;
00316
00317
00318
00319 control.name = "move_x";
00320 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00321 int_marker.controls.push_back(control);
00322
00323 control.orientation.w = 1;
00324 control.orientation.x = 0;
00325 control.orientation.y = 1;
00326 control.orientation.z = 0;
00327
00328
00329
00330 control.name = "move_z";
00331 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00332 int_marker.controls.push_back(control);
00333
00334 control.orientation.w = 1;
00335 control.orientation.x = 0;
00336 control.orientation.y = 0;
00337 control.orientation.z = 1;
00338
00339
00340
00341 control.name = "move_y";
00342 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00343 int_marker.controls.push_back(control);
00344
00345 server->insert(int_marker);
00346 server->setCallback(int_marker.name, &processFeedback);
00347 }
00348
00349
00350
00351 void makeRandomDofMarker( )
00352 {
00353 InteractiveMarker int_marker;
00354 int_marker.header.frame_id = "/base_link";
00355 int_marker.pose.position.y = -3.0 * marker_pos++;;
00356 int_marker.scale = 1;
00357
00358 int_marker.name = "6dof_random_axes";
00359 int_marker.description = "6-DOF\n(Arbitrary Axes)";
00360
00361 makeBoxControl(int_marker);
00362
00363 InteractiveMarkerControl control;
00364
00365 for ( int i=0; i<3; i++ )
00366 {
00367 control.orientation.w = rand(-1,1);
00368 control.orientation.x = rand(-1,1);
00369 control.orientation.y = rand(-1,1);
00370 control.orientation.z = rand(-1,1);
00371 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00372 int_marker.controls.push_back(control);
00373 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00374 int_marker.controls.push_back(control);
00375 }
00376
00377 server->insert(int_marker);
00378 server->setCallback(int_marker.name, &processFeedback);
00379 }
00380
00381
00382
00383
00384 void makeViewFacingMarker( )
00385 {
00386 InteractiveMarker int_marker;
00387 int_marker.header.frame_id = "/base_link";
00388 int_marker.pose.position.y = -3.0 * marker_pos++;;
00389 int_marker.scale = 1;
00390
00391 int_marker.name = "view_facing";
00392 int_marker.description = "View Facing 6-DOF";
00393
00394 InteractiveMarkerControl control;
00395
00396
00397 control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00398 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00399 control.orientation.w = 1;
00400 control.name = "rotate";
00401
00402 int_marker.controls.push_back(control);
00403
00404
00405
00406 control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00407 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00408 control.independent_marker_orientation = true;
00409 control.name = "move";
00410
00411 control.markers.push_back( makeBox(int_marker) );
00412 control.always_visible = true;
00413
00414 int_marker.controls.push_back(control);
00415
00416 server->insert(int_marker);
00417 server->setCallback(int_marker.name, &processFeedback);
00418 }
00419
00420
00421
00422
00423 void makeQuadrocopterMarker( )
00424 {
00425 InteractiveMarker int_marker;
00426 int_marker.header.frame_id = "/base_link";
00427 int_marker.pose.position.y = -3.0 * marker_pos++;;
00428 int_marker.scale = 1;
00429
00430 int_marker.name = "quadrocopter";
00431 int_marker.description = "Quadrocopter";
00432
00433 makeBoxControl(int_marker);
00434
00435 InteractiveMarkerControl control;
00436
00437 control.orientation.w = 1;
00438 control.orientation.x = 0;
00439 control.orientation.y = 1;
00440 control.orientation.z = 0;
00441 control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00442 int_marker.controls.push_back(control);
00443 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00444 int_marker.controls.push_back(control);
00445
00446 server->insert(int_marker);
00447 server->setCallback(int_marker.name, &processFeedback);
00448 }
00449
00450
00451
00452 void makeChessPieceMarker( )
00453 {
00454 InteractiveMarker int_marker;
00455 int_marker.header.frame_id = "/base_link";
00456 int_marker.pose.position.y = -3.0 * marker_pos++;;
00457 int_marker.scale = 1;
00458
00459 int_marker.name = "chess_piece";
00460 int_marker.description = "Chess Piece\n(2D Move + Alignment)";
00461
00462 InteractiveMarkerControl control;
00463
00464 control.orientation.w = 1;
00465 control.orientation.x = 0;
00466 control.orientation.y = 1;
00467 control.orientation.z = 0;
00468 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00469 int_marker.controls.push_back(control);
00470
00471
00472 control.markers.push_back( makeBox(int_marker) );
00473 control.always_visible = true;
00474 int_marker.controls.push_back(control);
00475
00476
00477 server->insert(int_marker);
00478 server->setCallback(int_marker.name, &processFeedback);
00479
00480
00481 server->setCallback(int_marker.name, &alignMarker, visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE );
00482 }
00483
00484
00485
00486 void makePanTiltMarker( )
00487 {
00488 InteractiveMarker int_marker;
00489 int_marker.header.frame_id = "/atc/laser/roof_rotating/base";
00490
00491 int_marker.pose.position.x = 0;
00492 int_marker.pose.position.y = 0;
00493 int_marker.pose.position.z = 0; int_marker.scale = 0.4;
00494
00495 int_marker.name = "shaft";
00496 int_marker.description = " Laser3D control";
00497
00498
00499
00500 InteractiveMarkerControl control;
00501
00502 control.orientation.w = 1;
00503 control.orientation.x = 1;
00504 control.orientation.y = 0;
00505 control.orientation.z = 0;
00506 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00507 control.orientation_mode = InteractiveMarkerControl::FIXED;
00508 int_marker.controls.push_back(control);
00509
00510 server->insert(int_marker);
00511 server->setCallback(int_marker.name, &processFeedback);
00512 }
00513
00514
00515
00516 void makeMenuMarker()
00517 {
00518 InteractiveMarker int_marker;
00519 int_marker.header.frame_id = "/atc/laser/roof_rotating/base";
00520
00521 int_marker.pose.position.x = 0;
00522 int_marker.pose.position.y = 0;
00523 int_marker.pose.position.z = 0;
00524 int_marker.scale = 0.4;
00525
00526 int_marker.name = "context_menu";
00527 int_marker.description = "Context Menu\n(Right Click)";
00528
00529 InteractiveMarkerControl control;
00530
00531
00532 control.interaction_mode = InteractiveMarkerControl::MENU;
00533 control.description="Options";
00534 control.name = "menu_only_control";
00535 int_marker.controls.push_back(control);
00536
00537
00538
00539
00540 control.always_visible = true;
00541 int_marker.controls.push_back(control);
00542
00543 server->insert(int_marker);
00544 server->setCallback(int_marker.name, &processFeedback);
00545
00546
00547 menu_handler.apply( *server, int_marker.name );
00548 }
00549
00550
00551
00552 void makeMovingMarker()
00553 {
00554 InteractiveMarker int_marker;
00555 int_marker.header.frame_id = "/moving_frame";
00556 int_marker.pose.position.y = -3.0 * marker_pos++;;
00557 int_marker.scale = 1;
00558
00559 int_marker.name = "moving";
00560 int_marker.description = "Marker Attached to a\nMoving Frame";
00561
00562 InteractiveMarkerControl control;
00563
00564 control.orientation.w = 1;
00565 control.orientation.x = 1;
00566 control.orientation.y = 0;
00567 control.orientation.z = 0;
00568 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00569 int_marker.controls.push_back(control);
00570
00571 control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00572 control.always_visible = true;
00573 control.markers.push_back( makeBox(int_marker) );
00574 int_marker.controls.push_back(control);
00575
00576 server->insert(int_marker);
00577 server->setCallback(int_marker.name, &processFeedback);
00578 }
00579
00580
00581
00582 int main(int argc, char** argv)
00583 {
00584 ros::init(argc, argv, "laser3d_interactive_control_node");
00585 ros::NodeHandle n;
00586
00587
00588 pub = n.advertise<sensor_msgs::JointState>("cmd", 1);
00589
00590
00591
00592
00593
00594 server.reset( new interactive_markers::InteractiveMarkerServer("imarker","",false) );
00595
00596 ros::Duration(0.1).sleep();
00597
00598
00599 menu_handler.insert( "Position control", &processFeedback );
00600 interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Speed control" );
00601 menu_handler.insert( sub_menu_handle, "30 RPM", &processFeedback );
00602 menu_handler.insert( sub_menu_handle, "60 RPM", &processFeedback );
00603
00604
00605
00606
00607
00608
00609
00610 makePanTiltMarker( );
00611 makeMenuMarker( );
00612
00613
00614 server->applyChanges();
00615
00616 ros::spin();
00617
00618 server.reset();
00619 }
00620
00621
00622
00623