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 #include <srs_interaction_primitives/object.h>
00029
00030 using namespace std;
00031 using namespace interactive_markers;
00032 using namespace visualization_msgs;
00033 using namespace geometry_msgs;
00034 using namespace std_msgs;
00035
00036 namespace srs_interaction_primitives
00037 {
00038
00039 Object::Object(InteractiveMarkerServerPtr server, string frame_id, string name) :
00040 BoundingBox(server, frame_id, name), show_pregrasp_control_(false), use_material_(false), translated_(false), allow_object_interaction_(
00041 true)
00042 {
00043 setPrimitiveType(srs_interaction_primitives::PrimitiveType::OBJECT);
00044
00045 if (ros::param::has(MoveArmToPregraspOnClick_PARAM))
00046 ros::param::get(MoveArmToPregraspOnClick_PARAM, move_arm_to_pregrasp_onclick_);
00047 else
00048 move_arm_to_pregrasp_onclick_ = false;
00049
00050 pregrasp1_.name = "control_grasp_xp";
00051 pregrasp2_.name = "control_grasp_xm";
00052 pregrasp3_.name = "control_grasp_yp";
00053 pregrasp4_.name = "control_grasp_ym";
00054 pregrasp5_.name = "control_grasp_zp";
00055 pregrasp6_.name = "control_grasp_zm";
00056
00057 if (pose_type_ == PoseType::POSE_BASE)
00058 {
00059 pose_.position.z += scale_.z * 0.5;
00060 }
00061 }
00062
00063 void Object::setAllowObjectInteraction(bool allow)
00064 {
00065 allow_object_interaction_ = allow;
00066 if (allow_object_interaction_)
00067 {
00068 menu_handler_.setVisible(menu_handler_interaction_, true);
00069 menu_handler_.setVisible(menu_handler_interaction_movement_, true);
00070 menu_handler_.setVisible(menu_handler_interaction_rotation_, true);
00071 }
00072 else
00073 {
00074 menu_handler_.setVisible(menu_handler_interaction_, false);
00075 menu_handler_.setVisible(menu_handler_interaction_movement_, false);
00076 menu_handler_.setVisible(menu_handler_interaction_rotation_, false);
00077 removeMovementControls();
00078 removeRotationControls();
00079 server_->insert(object_);
00080 }
00081
00082 menu_handler_.reApply(*server_);
00083 server_->applyChanges();
00084 }
00085
00086 void Object::objectCallback(const InteractiveMarkerFeedbackConstPtr &feedback)
00087 {
00088 if (move_arm_to_pregrasp_onclick_ && feedback->event_type == InteractiveMarkerFeedback::MOUSE_DOWN)
00089 {
00090 if (feedback->control_name == pregrasp1_.name)
00091 updatePublisher_->publishMoveArmToPreGrasp(1);
00092 else if (feedback->control_name == pregrasp2_.name)
00093 updatePublisher_->publishMoveArmToPreGrasp(2);
00094 else if (feedback->control_name == pregrasp3_.name)
00095 updatePublisher_->publishMoveArmToPreGrasp(3);
00096 else if (feedback->control_name == pregrasp4_.name)
00097 updatePublisher_->publishMoveArmToPreGrasp(4);
00098 else if (feedback->control_name == pregrasp5_.name)
00099 updatePublisher_->publishMoveArmToPreGrasp(5);
00100 else if (feedback->control_name == pregrasp6_.name)
00101 updatePublisher_->publishMoveArmToPreGrasp(6);
00102 }
00103
00104 defaultCallback(feedback);
00105 }
00106
00107 void Object::menuCallback(const InteractiveMarkerFeedbackConstPtr &feedback)
00108 {
00109 MenuHandler::EntryHandle handle = feedback->menu_entry_id;
00110 MenuHandler::CheckState state;
00111 string title;
00112 menu_handler_.getCheckState(handle, state);
00113 menu_handler_.getTitle(handle, title);
00114
00115 updatePublisher_->publishMenuClicked(title, state);
00116
00117
00118
00119
00120
00121
00122
00123
00124 switch (feedback->menu_entry_id)
00125 {
00126 case 1:
00127
00128
00129
00130 if (state == MenuHandler::CHECKED)
00131 {
00132 showBoundingBoxControl(false);
00133 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00134 }
00135 else
00136 {
00137 showBoundingBoxControl(true);
00138 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00139 }
00140 break;
00141 case 2:
00142
00143
00144
00145 if (state == MenuHandler::CHECKED)
00146 {
00147 removeDescriptionControl();
00148 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00149 }
00150 else
00151 {
00152 addDescriptionControl();
00153 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00154 }
00155 break;
00156 case 3:
00157
00158
00159
00160 if (state == MenuHandler::CHECKED)
00161 {
00162 removeMeasureControl();
00163 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00164 }
00165 else
00166 {
00167 addMeasureControl();
00168 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00169 }
00170 break;
00171 case 4:
00172
00173
00174
00175 if (state == MenuHandler::CHECKED)
00176 {
00177 removePregraspPositions();
00178 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00179 }
00180 else
00181 {
00182 addPregraspPositions();
00183 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00184 }
00185 break;
00186 case 5:
00187
00188
00189
00190 if (state == MenuHandler::CHECKED)
00191 {
00192 move_arm_to_pregrasp_onclick_ = false;
00193 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00194 }
00195 else
00196 {
00197 move_arm_to_pregrasp_onclick_ = true;
00198 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00199 }
00200 break;
00201
00202
00203 case 7:
00204
00205
00206
00207 addMovementControls();
00208 addRotationControls();
00209 menu_handler_.setCheckState(menu_handler_interaction_movement_, MenuHandler::CHECKED);
00210 menu_handler_.setCheckState(menu_handler_interaction_rotation_, MenuHandler::CHECKED);
00211 break;
00212 case 8:
00213
00214
00215
00216 removeMovementControls();
00217 removeRotationControls();
00218 menu_handler_.setCheckState(menu_handler_interaction_movement_, MenuHandler::UNCHECKED);
00219 menu_handler_.setCheckState(menu_handler_interaction_rotation_, MenuHandler::UNCHECKED);
00220 break;
00221
00222 case 9:
00223
00224
00225
00226 if (state == MenuHandler::CHECKED)
00227 {
00228 removeMovementControls();
00229 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00230 }
00231 else
00232 {
00233 addMovementControls();
00234 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00235 }
00236 break;
00237 case 10:
00238
00239
00240
00241 if (state == MenuHandler::CHECKED)
00242 {
00243 removeRotationControls();
00244 menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00245 }
00246 else
00247 {
00248 addRotationControls();
00249 menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00250 }
00251 break;
00252 }
00253
00254 server_->insert(object_);
00255 menu_handler_.reApply(*server_);
00256 server_->applyChanges();
00257 }
00258
00259 void Object::createMenu()
00260 {
00261 if (!menu_created_)
00262 {
00263 menu_created_ = true;
00264 menu_handler_.setCheckState(menu_handler_.insert("Show bounding box", boost::bind(&Object::menuCallback, this, _1)),
00265 MenuHandler::CHECKED);
00266 menu_handler_.setCheckState(menu_handler_.insert("Show description", boost::bind(&Object::menuCallback, this, _1)),
00267 MenuHandler::UNCHECKED);
00268 menu_handler_.setCheckState(menu_handler_.insert("Show measure", boost::bind(&Object::menuCallback, this, _1)),
00269 MenuHandler::UNCHECKED);
00270
00271 bool show_pregrasp = false;
00272 if (ros::param::has(ShowPregrasp_PARAM))
00273 ros::param::get(ShowPregrasp_PARAM, show_pregrasp);
00274 menu_handler_show_pregrasp_ = menu_handler_.insert("Show pre-grasp positions",
00275 boost::bind(&Object::menuCallback, this, _1));
00276 if (show_pregrasp)
00277 {
00278 menu_handler_.setCheckState(menu_handler_show_pregrasp_, MenuHandler::CHECKED);
00279 addPregraspPositions();
00280 }
00281 else
00282 menu_handler_.setCheckState(menu_handler_show_pregrasp_, MenuHandler::UNCHECKED);
00283
00284 menu_handler_move_to_pregrasp_ = menu_handler_.insert("Move arm to pre-grasp position on click",
00285 boost::bind(&Object::menuCallback, this, _1));
00286 if (move_arm_to_pregrasp_onclick_)
00287 menu_handler_.setCheckState(menu_handler_move_to_pregrasp_, MenuHandler::CHECKED);
00288 else
00289 menu_handler_.setCheckState(menu_handler_move_to_pregrasp_, MenuHandler::UNCHECKED);
00290
00291
00292
00293
00294 menu_handler_interaction_ = menu_handler_.insert("Interaction");
00295 menu_handler_.setCheckState(
00296 menu_handler_.insert(menu_handler_interaction_, "Enable All", boost::bind(&Object::menuCallback, this, _1)),
00297 MenuHandler::NO_CHECKBOX);
00298 menu_handler_.setCheckState(
00299 menu_handler_.insert(menu_handler_interaction_, "Disable All", boost::bind(&Object::menuCallback, this, _1)),
00300 MenuHandler::NO_CHECKBOX);
00301
00302 menu_handler_interaction_movement_ = menu_handler_.insert(menu_handler_interaction_, "Movement",
00303 boost::bind(&Object::menuCallback, this, _1));
00304 menu_handler_interaction_rotation_ = menu_handler_.insert(menu_handler_interaction_, "Rotation",
00305 boost::bind(&Object::menuCallback, this, _1));
00306
00307 menu_handler_.setCheckState(menu_handler_interaction_movement_, MenuHandler::UNCHECKED);
00308 menu_handler_.setCheckState(menu_handler_interaction_rotation_, MenuHandler::UNCHECKED);
00309
00310 if (!allow_object_interaction_)
00311 {
00312 menu_handler_.setVisible(menu_handler_interaction_, false);
00313 menu_handler_.setVisible(menu_handler_interaction_movement_, false);
00314 menu_handler_.setVisible(menu_handler_interaction_rotation_, false);
00315 }
00316 if (!allow_pregrasp_)
00317 {
00318 menu_handler_.setVisible(menu_handler_show_pregrasp_, false);
00319 menu_handler_.setVisible(menu_handler_move_to_pregrasp_, false);
00320 }
00321 }
00322 }
00323
00324 void Object::addPregraspPositions()
00325 {
00326 show_pregrasp_control_ = true;
00327
00328 Marker arrow;
00329 arrow.color = color_green_a01_;
00330 arrow.color.a = GRASP_TRANSPARENCY;
00331 arrow.scale.x = GRASP_ARROW_LENGTH;
00332 arrow.scale.y = GRASP_ARROW_WIDTH;
00333 arrow.scale.z = GRASP_ARROW_WIDTH;
00334
00335 Marker text;
00336 text.type = Marker::TEXT_VIEW_FACING;
00337 text.scale.z = GRASP_TEXT_SIZE;
00338 text.color = color_green_a01_;
00339 text.color.a = GRASP_TRANSPARENCY;
00340
00341 Marker point;
00342 point.type = Marker::SPHERE;
00343 point.scale.x = GRASP_POINT_SCALE;
00344 point.scale.y = GRASP_POINT_SCALE;
00345 point.scale.z = GRASP_POINT_SCALE;
00346 point.color.r = 1;
00347 point.color.g = 1;
00348 point.color.b = 0;
00349 point.color.a = arrow.color.a = GRASP_TRANSPARENCY;
00350
00351 pregrasp1Control_.markers.clear();
00352 pregrasp2Control_.markers.clear();
00353 pregrasp3Control_.markers.clear();
00354 pregrasp4Control_.markers.clear();
00355 pregrasp5Control_.markers.clear();
00356 pregrasp6Control_.markers.clear();
00357
00358 if (pregrasp1_.enabled)
00359 {
00360 pregrasp1Control_.name = pregrasp1_.name;
00361 pregrasp1Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00362
00363 arrow.pose = pregrasp1_.pose;
00364 pregrasp1Control_.markers.push_back(arrow);
00365
00366 text.pose.position.x = pregrasp1_.pose.position.x;
00367 text.pose.position.y = pregrasp1_.pose.position.y;
00368 text.pose.position.z = pregrasp1_.pose.position.z + GRASP_TEXT_OFFSET;
00369 text.text = "1";
00370 pregrasp1Control_.markers.push_back(text);
00371
00372 point.pose = pregrasp1_.pose;
00373 pregrasp1Control_.markers.push_back(point);
00374
00375 object_.controls.push_back(pregrasp1Control_);
00376 }
00377
00378 if (pregrasp2_.enabled)
00379 {
00380 pregrasp2Control_.name = pregrasp2_.name;
00381 pregrasp2Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00382
00383 arrow.pose = pregrasp2_.pose;
00384 pregrasp2Control_.markers.push_back(arrow);
00385
00386 text.pose.position.x = pregrasp2_.pose.position.x;
00387 text.pose.position.y = pregrasp2_.pose.position.y;
00388 text.pose.position.z = pregrasp2_.pose.position.z + GRASP_TEXT_OFFSET;
00389 text.text = "2";
00390 pregrasp2Control_.markers.push_back(text);
00391
00392 point.pose = pregrasp2_.pose;
00393 pregrasp2Control_.markers.push_back(point);
00394
00395 object_.controls.push_back(pregrasp2Control_);
00396 }
00397
00398 if (pregrasp3_.enabled)
00399 {
00400 pregrasp3Control_.name = pregrasp3_.name;
00401 pregrasp3Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00402
00403 arrow.pose = pregrasp3_.pose;
00404 pregrasp3Control_.markers.push_back(arrow);
00405
00406 text.pose.position.x = pregrasp3_.pose.position.x;
00407 text.pose.position.y = pregrasp3_.pose.position.y;
00408 text.pose.position.z = pregrasp3_.pose.position.z + GRASP_TEXT_OFFSET;
00409 text.text = "3";
00410 pregrasp3Control_.markers.push_back(text);
00411
00412 point.pose = pregrasp3_.pose;
00413 pregrasp3Control_.markers.push_back(point);
00414
00415 object_.controls.push_back(pregrasp3Control_);
00416 }
00417
00418 if (pregrasp4_.enabled)
00419 {
00420 pregrasp4Control_.name = pregrasp4_.name;
00421 pregrasp4Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00422
00423 arrow.pose = pregrasp4_.pose;
00424 pregrasp4Control_.markers.push_back(arrow);
00425
00426 text.pose.position.x = pregrasp4_.pose.position.x;
00427 text.pose.position.y = pregrasp4_.pose.position.y;
00428 text.pose.position.z = pregrasp4_.pose.position.z + GRASP_TEXT_OFFSET;
00429 text.text = "4";
00430 pregrasp4Control_.markers.push_back(text);
00431
00432 point.pose = pregrasp4_.pose;
00433 pregrasp4Control_.markers.push_back(point);
00434
00435 object_.controls.push_back(pregrasp4Control_);
00436 }
00437
00438 if (pregrasp5_.enabled)
00439 {
00440 pregrasp5Control_.name = pregrasp5_.name;
00441 pregrasp5Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00442
00443 arrow.pose = pregrasp5_.pose;
00444 pregrasp5Control_.markers.push_back(arrow);
00445
00446 text.pose.position.x = pregrasp5_.pose.position.x;
00447 text.pose.position.y = pregrasp5_.pose.position.y;
00448 text.pose.position.z = pregrasp5_.pose.position.z + GRASP_TEXT_OFFSET;
00449 text.text = "5";
00450 pregrasp5Control_.markers.push_back(text);
00451
00452 point.pose = pregrasp5_.pose;
00453 pregrasp5Control_.markers.push_back(point);
00454
00455 object_.controls.push_back(pregrasp5Control_);
00456 }
00457
00458 if (pregrasp6_.enabled)
00459 {
00460 pregrasp6Control_.name = pregrasp6_.name;
00461 pregrasp6Control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00462
00463 arrow.pose = pregrasp6_.pose;
00464 pregrasp6Control_.markers.push_back(arrow);
00465
00466 text.pose.position.x = pregrasp6_.pose.position.x;
00467 text.pose.position.y = pregrasp6_.pose.position.y;
00468 text.pose.position.z = pregrasp6_.pose.position.z + GRASP_TEXT_OFFSET;
00469 text.text = "6";
00470 pregrasp6Control_.markers.push_back(text);
00471
00472 point.pose = pregrasp6_.pose;
00473 pregrasp6Control_.markers.push_back(point);
00474
00475 object_.controls.push_back(pregrasp6Control_);
00476 }
00477
00478 }
00479
00480 void Object::removePregraspPositions()
00481 {
00482 show_pregrasp_control_ = false;
00483 removeControl(pregrasp1_.name);
00484 removeControl(pregrasp2_.name);
00485 removeControl(pregrasp3_.name);
00486 removeControl(pregrasp4_.name);
00487 removeControl(pregrasp5_.name);
00488 removeControl(pregrasp6_.name);
00489 }
00490
00491 void Object::addPreGraspPosition(int pos_id, Pose pose)
00492 {
00493 switch (pos_id)
00494 {
00495 case GRASP_1:
00496 pregrasp1_.enabled = true;
00497 pregrasp1_.pose = pose;
00498 break;
00499 case GRASP_2:
00500 pregrasp2_.enabled = true;
00501 pregrasp2_.pose = pose;
00502 break;
00503 case GRASP_3:
00504 pregrasp3_.enabled = true;
00505 pregrasp3_.pose = pose;
00506 break;
00507 case GRASP_4:
00508 pregrasp4_.enabled = true;
00509 pregrasp4_.pose = pose;
00510 break;
00511 case GRASP_5:
00512 pregrasp5_.enabled = true;
00513 pregrasp5_.pose = pose;
00514 break;
00515 case GRASP_6:
00516 pregrasp6_.enabled = true;
00517 pregrasp6_.pose = pose;
00518 break;
00519 default:
00520 ROS_WARN("Unknown pre-grasp position");
00521 return;
00522 break;
00523 }
00524 }
00525
00526 void Object::updateControls()
00527 {
00528 if (show_pregrasp_control_)
00529 {
00530 removePregraspPositions();
00531 addPregraspPositions();
00532 }
00533 Primitive::updateControls();
00534 }
00535
00536 void Object::removePreGraspPosition(int pos_id)
00537 {
00538 switch (pos_id)
00539 {
00540 case GRASP_1:
00541 pregrasp1_.enabled = false;
00542 break;
00543 case GRASP_2:
00544 pregrasp2_.enabled = false;
00545 break;
00546 case GRASP_3:
00547 pregrasp3_.enabled = false;
00548 break;
00549 case GRASP_4:
00550 pregrasp4_.enabled = false;
00551 break;
00552 case GRASP_5:
00553 pregrasp5_.enabled = false;
00554 break;
00555 case GRASP_6:
00556 pregrasp6_.enabled = false;
00557 break;
00558 default:
00559 ROS_WARN("Unknown pre-grasp position");
00560 return;
00561 break;
00562 }
00563 updateControls();
00564 }
00565
00566 void Object::createMesh()
00567 {
00568 mesh_.pose.position.z = -0.5 * scale_.z;
00569
00570 mesh_.color = color_;
00571 mesh_.scale = Vector3();
00572 switch (object_resource_)
00573 {
00574 case RESOURCE_FILE:
00575 mesh_.type = Marker::MESH_RESOURCE;
00576 mesh_.mesh_use_embedded_materials = use_material_;
00577 mesh_.mesh_resource = resource_;
00578 break;
00579 case SHAPE:
00580 mesh_.type = Marker::TRIANGLE_LIST;
00581 for (unsigned int i = 0; i < shape_.triangles.size(); i++)
00582 mesh_.points.push_back(shape_.vertices[shape_.triangles[i]]);
00583 break;
00584 }
00585 }
00586
00587 void Object::setPoseLWH(Pose pose, Point bounding_box_lwh)
00588 {
00589 bounding_box_lwh_ = bounding_box_lwh;
00590 scale_.x = bounding_box_lwh_.x;
00591 scale_.y = bounding_box_lwh_.y;
00592 scale_.z = bounding_box_lwh_.z;
00593
00594 setPose(pose);
00595 }
00596
00597 void Object::create()
00598 {
00599 clearObject();
00600
00601 scale_.x = bounding_box_lwh_.x;
00602 scale_.y = bounding_box_lwh_.y;
00603 scale_.z = bounding_box_lwh_.z;
00604
00605 object_.header.frame_id = frame_id_;
00606 object_.name = name_;
00607
00608 object_.pose = pose_;
00609 object_.scale = maxScale(scale_);
00610
00611 createMesh();
00612
00613 control_.name = "object_control";
00614 control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00615 control_.always_visible = true;
00616 control_.markers.push_back(mesh_);
00617
00618 object_.controls.clear();
00619 object_.controls.push_back(control_);
00620
00621 if (scale_.x != 0.0f && scale_.y != 0.0f && scale_.z != 0.0f)
00622 BoundingBox::createBoundingBoxControl(0.0f, 0.0f, 0.0f);
00623
00624 createMenu();
00625 }
00626
00627 void Object::insert()
00628 {
00629 create();
00630 updateControls();
00631 server_->insert(object_, boost::bind(&Object::objectCallback, this, _1));
00632 menu_handler_.apply(*server_, name_);
00633 }
00634
00635 }