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