motion_planning_frame_objects.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Mario Prats */
00036 
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 
00039 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00040 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
00041 #include <moveit/robot_state/conversions.h>
00042 #include <moveit/robot_interaction/interactive_marker_helpers.h>
00043 
00044 #include <interactive_markers/tools.h>
00045 
00046 #include <rviz/display_context.h>
00047 #include <rviz/frame_manager.h>
00048 #include <rviz/window_manager_interface.h>
00049 
00050 #include <eigen_conversions/eigen_msg.h>
00051 #include <geometric_shapes/shape_operations.h>
00052 
00053 #include <QMessageBox>
00054 #include <QInputDialog>
00055 #include <QFileDialog>
00056 
00057 #include "ui_motion_planning_rviz_plugin_frame.h"
00058 
00059 namespace moveit_rviz_plugin
00060 {
00061 void MotionPlanningFrame::importFileButtonClicked()
00062 {
00063   QString path = QFileDialog::getOpenFileName(this, tr("Import Object"));
00064   if (!path.isEmpty())
00065     importResource("file://" + path.toStdString());
00066 }
00067 
00068 void MotionPlanningFrame::importUrlButtonClicked()
00069 {
00070   bool ok = false;
00071   QString url = QInputDialog::getText(this, tr("Import Object"), tr("URL for file to import:"), QLineEdit::Normal,
00072                                       QString("http://"), &ok);
00073   if (ok && !url.isEmpty())
00074     importResource(url.toStdString());
00075 }
00076 
00077 void MotionPlanningFrame::clearSceneButtonClicked()
00078 {
00079   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00080   if (ps)
00081   {
00082     ps->getWorldNonConst()->clearObjects();
00083     ps->getCurrentStateNonConst().clearAttachedBodies();
00084     moveit_msgs::PlanningScene msg;
00085     ps->getPlanningSceneMsg(msg);
00086     planning_scene_publisher_.publish(msg);
00087     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
00088     planning_display_->queueRenderSceneGeometry();
00089   }
00090 }
00091 
00092 void MotionPlanningFrame::sceneScaleChanged(int value)
00093 {
00094   if (scaled_object_)
00095   {
00096     planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00097     if (ps)
00098     {
00099       if (ps->getWorld()->hasObject(scaled_object_->id_))
00100       {
00101         ps->getWorldNonConst()->removeObject(scaled_object_->id_);
00102         for (std::size_t i = 0; i < scaled_object_->shapes_.size(); ++i)
00103         {
00104           shapes::Shape* s = scaled_object_->shapes_[i]->clone();
00105           s->scale((double)value / 100.0);
00106           ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s),
00107                                               scaled_object_->shape_poses_[i]);
00108         }
00109         planning_display_->queueRenderSceneGeometry();
00110       }
00111       else
00112         scaled_object_.reset();
00113     }
00114     else
00115       scaled_object_.reset();
00116   }
00117 }
00118 
00119 void MotionPlanningFrame::sceneScaleStartChange()
00120 {
00121   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00122   if (sel.empty())
00123     return;
00124   if (planning_display_->getPlanningSceneMonitor() && sel[0]->checkState() == Qt::Unchecked)
00125   {
00126     planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00127     if (ps)
00128     {
00129       scaled_object_ = ps->getWorld()->getObject(sel[0]->text().toStdString());
00130     }
00131   }
00132 }
00133 
00134 void MotionPlanningFrame::sceneScaleEndChange()
00135 {
00136   scaled_object_.reset();
00137   ui_->scene_scale->setSliderPosition(100);
00138 }
00139 
00140 void MotionPlanningFrame::removeObjectButtonClicked()
00141 {
00142   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00143   if (sel.empty())
00144     return;
00145   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00146   if (ps)
00147   {
00148     for (int i = 0; i < sel.count(); ++i)
00149       if (sel[i]->checkState() == Qt::Unchecked)
00150         ps->getWorldNonConst()->removeObject(sel[i]->text().toStdString());
00151       else
00152         ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString());
00153     scene_marker_.reset();
00154     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
00155     planning_display_->queueRenderSceneGeometry();
00156   }
00157 }
00158 
00159 static QString decideStatusText(const collision_detection::CollisionWorld::ObjectConstPtr& obj)
00160 {
00161   QString status_text = "'" + QString::fromStdString(obj->id_) + "' is a collision object with ";
00162   if (obj->shapes_.empty())
00163     status_text += "no geometry";
00164   else
00165   {
00166     std::vector<QString> shape_names;
00167     for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
00168       shape_names.push_back(QString::fromStdString(shapes::shapeStringName(obj->shapes_[i].get())));
00169     if (shape_names.size() == 1)
00170       status_text += "one " + shape_names[0];
00171     else
00172     {
00173       status_text += QString::fromStdString(boost::lexical_cast<std::string>(shape_names.size())) + " shapes:";
00174       for (std::size_t i = 0; i < shape_names.size(); ++i)
00175         status_text += " " + shape_names[i];
00176     }
00177   }
00178   return status_text;
00179 }
00180 
00181 static QString decideStatusText(const robot_state::AttachedBody* attached_body)
00182 {
00183   QString status_text = "'" + QString::fromStdString(attached_body->getName()) + "' is attached to '" +
00184                         QString::fromStdString(attached_body->getAttachedLinkName()) + "'";
00185   return status_text;
00186 }
00187 
00188 void MotionPlanningFrame::selectedCollisionObjectChanged()
00189 {
00190   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00191   if (sel.empty())
00192   {
00193     bool oldState = ui_->object_x->blockSignals(true);
00194     ui_->object_x->setValue(0.0);
00195     ui_->object_x->blockSignals(oldState);
00196 
00197     oldState = ui_->object_y->blockSignals(true);
00198     ui_->object_y->setValue(0.0);
00199     ui_->object_y->blockSignals(oldState);
00200 
00201     oldState = ui_->object_z->blockSignals(true);
00202     ui_->object_z->setValue(0.0);
00203     ui_->object_z->blockSignals(oldState);
00204 
00205     oldState = ui_->object_rx->blockSignals(true);
00206     ui_->object_rx->setValue(0.0);
00207     ui_->object_rx->blockSignals(oldState);
00208 
00209     oldState = ui_->object_ry->blockSignals(true);
00210     ui_->object_ry->setValue(0.0);
00211     ui_->object_ry->blockSignals(oldState);
00212 
00213     oldState = ui_->object_rz->blockSignals(true);
00214     ui_->object_rz->setValue(0.0);
00215     ui_->object_rz->blockSignals(oldState);
00216 
00217     ui_->object_status->setText("");
00218     scene_marker_.reset();
00219     ui_->scene_scale->setEnabled(false);
00220   }
00221   else if (planning_display_->getPlanningSceneMonitor())
00222   {
00223     // if this is a CollisionWorld element
00224     if (sel[0]->checkState() == Qt::Unchecked)
00225     {
00226       ui_->scene_scale->setEnabled(true);
00227       bool update_scene_marker = false;
00228       Eigen::Affine3d obj_pose;
00229       {
00230         const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00231         const collision_detection::CollisionWorld::ObjectConstPtr& obj =
00232             ps->getWorld()->getObject(sel[0]->text().toStdString());
00233         if (obj)
00234         {
00235           ui_->object_status->setText(decideStatusText(obj));
00236 
00237           if (obj->shapes_.size() == 1)
00238           {
00239             obj_pose = obj->shape_poses_[0];
00240             Eigen::Vector3d xyz = obj_pose.rotation().eulerAngles(0, 1, 2);
00241             update_scene_marker = true;  // do the marker update outside locked scope to avoid deadlock
00242 
00243             bool oldState = ui_->object_x->blockSignals(true);
00244             ui_->object_x->setValue(obj_pose.translation()[0]);
00245             ui_->object_x->blockSignals(oldState);
00246 
00247             oldState = ui_->object_y->blockSignals(true);
00248             ui_->object_y->setValue(obj_pose.translation()[1]);
00249             ui_->object_y->blockSignals(oldState);
00250 
00251             oldState = ui_->object_z->blockSignals(true);
00252             ui_->object_z->setValue(obj_pose.translation()[2]);
00253             ui_->object_z->blockSignals(oldState);
00254 
00255             oldState = ui_->object_rx->blockSignals(true);
00256             ui_->object_rx->setValue(xyz[0]);
00257             ui_->object_rx->blockSignals(oldState);
00258 
00259             oldState = ui_->object_ry->blockSignals(true);
00260             ui_->object_ry->setValue(xyz[1]);
00261             ui_->object_ry->blockSignals(oldState);
00262 
00263             oldState = ui_->object_rz->blockSignals(true);
00264             ui_->object_rz->setValue(xyz[2]);
00265             ui_->object_rz->blockSignals(oldState);
00266           }
00267         }
00268         else
00269           ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be a collision object but it is not");
00270       }
00271       if (update_scene_marker && ui_->tabWidget->tabText(ui_->tabWidget->currentIndex()).toStdString() == TAB_OBJECTS)
00272       {
00273         createSceneInteractiveMarker();
00274       }
00275     }
00276     else
00277     {
00278       ui_->scene_scale->setEnabled(false);
00279       // if it is an attached object
00280       scene_marker_.reset();
00281       const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00282       const robot_state::AttachedBody* attached_body =
00283           ps->getCurrentState().getAttachedBody(sel[0]->text().toStdString());
00284       if (attached_body)
00285         ui_->object_status->setText(decideStatusText(attached_body));
00286       else
00287         ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be an attached object but it is not");
00288     }
00289   }
00290 }
00291 
00292 void MotionPlanningFrame::objectPoseValueChanged(double /* value */)
00293 {
00294   updateCollisionObjectPose(true);
00295 }
00296 
00297 void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
00298 {
00299   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00300   if (sel.empty())
00301     return;
00302   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00303   if (ps)
00304   {
00305     collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
00306     if (obj && obj->shapes_.size() == 1)
00307     {
00308       Eigen::Affine3d p;
00309       p.translation()[0] = ui_->object_x->value();
00310       p.translation()[1] = ui_->object_y->value();
00311       p.translation()[2] = ui_->object_z->value();
00312 
00313       p = Eigen::Translation3d(p.translation()) *
00314           (Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
00315            Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
00316            Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));
00317 
00318       ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p);
00319       planning_display_->queueRenderSceneGeometry();
00320 
00321       // Update the interactive marker pose to match the manually introduced one
00322       if (update_marker_position && scene_marker_)
00323       {
00324         Eigen::Quaterniond eq(p.rotation());
00325         scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
00326                                Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
00327       }
00328     }
00329   }
00330 }
00331 
00332 void MotionPlanningFrame::collisionObjectChanged(QListWidgetItem* item)
00333 {
00334   if (item->type() < (int)known_collision_objects_.size() && planning_display_->getPlanningSceneMonitor())
00335   {
00336     // if we have a name change
00337     if (known_collision_objects_[item->type()].first != item->text().toStdString())
00338       renameCollisionObject(item);
00339     else
00340     {
00341       bool checked = item->checkState() == Qt::Checked;
00342       if (known_collision_objects_[item->type()].second != checked)
00343         attachDetachCollisionObject(item);
00344     }
00345   }
00346 }
00347 
00348 /* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */
00349 void MotionPlanningFrame::imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00350 {
00351   bool oldState = ui_->object_x->blockSignals(true);
00352   ui_->object_x->setValue(feedback.pose.position.x);
00353   ui_->object_x->blockSignals(oldState);
00354 
00355   oldState = ui_->object_y->blockSignals(true);
00356   ui_->object_y->setValue(feedback.pose.position.y);
00357   ui_->object_y->blockSignals(oldState);
00358 
00359   oldState = ui_->object_z->blockSignals(true);
00360   ui_->object_z->setValue(feedback.pose.position.z);
00361   ui_->object_z->blockSignals(oldState);
00362 
00363   Eigen::Quaterniond q;
00364   tf::quaternionMsgToEigen(feedback.pose.orientation, q);
00365   Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);
00366 
00367   oldState = ui_->object_rx->blockSignals(true);
00368   ui_->object_rx->setValue(xyz[0]);
00369   ui_->object_rx->blockSignals(oldState);
00370 
00371   oldState = ui_->object_ry->blockSignals(true);
00372   ui_->object_ry->setValue(xyz[1]);
00373   ui_->object_ry->blockSignals(oldState);
00374 
00375   oldState = ui_->object_rz->blockSignals(true);
00376   ui_->object_rz->setValue(xyz[2]);
00377   ui_->object_rz->blockSignals(oldState);
00378 
00379   updateCollisionObjectPose(false);
00380 }
00381 
00382 void MotionPlanningFrame::copySelectedCollisionObject()
00383 {
00384   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00385   if (sel.empty())
00386     return;
00387 
00388   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00389   if (!ps)
00390     return;
00391 
00392   for (int i = 0; i < sel.size(); ++i)
00393   {
00394     std::string name = sel[i]->text().toStdString();
00395     collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(name);
00396     if (!obj)
00397       continue;
00398 
00399     // find a name for the copy
00400     name = "Copy of " + name;
00401     if (ps->getWorld()->hasObject(name))
00402     {
00403       name += " ";
00404       unsigned int n = 1;
00405       while (ps->getWorld()->hasObject(name + boost::lexical_cast<std::string>(n)))
00406         n++;
00407       name += boost::lexical_cast<std::string>(n);
00408     }
00409     ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_);
00410     ROS_DEBUG("Copied collision object to '%s'", name.c_str());
00411   }
00412   planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
00413 }
00414 
00415 void MotionPlanningFrame::computeSaveSceneButtonClicked()
00416 {
00417   if (planning_scene_storage_)
00418   {
00419     moveit_msgs::PlanningScene msg;
00420     planning_display_->getPlanningSceneRO()->getPlanningSceneMsg(msg);
00421     try
00422     {
00423       planning_scene_storage_->removePlanningScene(msg.name);
00424       planning_scene_storage_->addPlanningScene(msg);
00425     }
00426     catch (std::runtime_error& ex)
00427     {
00428       ROS_ERROR("%s", ex.what());
00429     }
00430 
00431     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
00432   }
00433 }
00434 
00435 void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name)
00436 {
00437   moveit_msgs::MotionPlanRequest mreq;
00438   constructPlanningRequest(mreq);
00439   if (planning_scene_storage_)
00440   {
00441     try
00442     {
00443       if (!query_name.empty())
00444         planning_scene_storage_->removePlanningQuery(scene, query_name);
00445       planning_scene_storage_->addPlanningQuery(mreq, scene, query_name);
00446     }
00447     catch (std::runtime_error& ex)
00448     {
00449       ROS_ERROR("%s", ex.what());
00450     }
00451 
00452     planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
00453   }
00454 }
00455 
00456 void MotionPlanningFrame::computeDeleteSceneButtonClicked()
00457 {
00458   if (planning_scene_storage_)
00459   {
00460     QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00461     if (!sel.empty())
00462     {
00463       QTreeWidgetItem* s = sel.front();
00464       if (s->type() == ITEM_TYPE_SCENE)
00465       {
00466         std::string scene = s->text(0).toStdString();
00467         try
00468         {
00469           planning_scene_storage_->removePlanningScene(scene);
00470         }
00471         catch (std::runtime_error& ex)
00472         {
00473           ROS_ERROR("%s", ex.what());
00474         }
00475       }
00476       else
00477       {
00478         // if we selected a query name, then we overwrite that query
00479         std::string scene = s->parent()->text(0).toStdString();
00480         try
00481         {
00482           planning_scene_storage_->removePlanningScene(scene);
00483         }
00484         catch (std::runtime_error& ex)
00485         {
00486           ROS_ERROR("%s", ex.what());
00487         }
00488       }
00489       planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
00490     }
00491   }
00492 }
00493 
00494 void MotionPlanningFrame::computeDeleteQueryButtonClicked()
00495 {
00496   if (planning_scene_storage_)
00497   {
00498     QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00499     if (!sel.empty())
00500     {
00501       QTreeWidgetItem* s = sel.front();
00502       if (s->type() == ITEM_TYPE_QUERY)
00503       {
00504         std::string scene = s->parent()->text(0).toStdString();
00505         std::string query_name = s->text(0).toStdString();
00506         try
00507         {
00508           planning_scene_storage_->removePlanningQuery(scene, query_name);
00509         }
00510         catch (std::runtime_error& ex)
00511         {
00512           ROS_ERROR("%s", ex.what());
00513         }
00514         planning_display_->addMainLoopJob(
00515             boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s));
00516       }
00517     }
00518   }
00519 }
00520 
00521 void MotionPlanningFrame::computeDeleteQueryButtonClickedHelper(QTreeWidgetItem* s)
00522 {
00523   ui_->planning_scene_tree->setUpdatesEnabled(false);
00524   s->parent()->removeChild(s);
00525   ui_->planning_scene_tree->setUpdatesEnabled(true);
00526 }
00527 
00528 void MotionPlanningFrame::checkPlanningSceneTreeEnabledButtons()
00529 {
00530   QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00531   if (sel.empty())
00532   {
00533     ui_->load_scene_button->setEnabled(false);
00534     ui_->load_query_button->setEnabled(false);
00535     ui_->save_query_button->setEnabled(false);
00536     ui_->delete_scene_button->setEnabled(false);
00537   }
00538   else
00539   {
00540     ui_->save_query_button->setEnabled(true);
00541 
00542     QTreeWidgetItem* s = sel.front();
00543 
00544     // if the item is a PlanningScene
00545     if (s->type() == ITEM_TYPE_SCENE)
00546     {
00547       ui_->load_scene_button->setEnabled(true);
00548       ui_->load_query_button->setEnabled(false);
00549       ui_->delete_scene_button->setEnabled(true);
00550       ui_->delete_query_button->setEnabled(false);
00551       ui_->save_query_button->setEnabled(true);
00552     }
00553     else
00554     {
00555       // if the item is a query
00556       ui_->load_scene_button->setEnabled(false);
00557       ui_->load_query_button->setEnabled(true);
00558       ui_->delete_scene_button->setEnabled(false);
00559       ui_->delete_query_button->setEnabled(true);
00560     }
00561   }
00562 }
00563 
00564 void MotionPlanningFrame::computeLoadSceneButtonClicked()
00565 {
00566   if (planning_scene_storage_)
00567   {
00568     QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00569     if (!sel.empty())
00570     {
00571       QTreeWidgetItem* s = sel.front();
00572       if (s->type() == ITEM_TYPE_SCENE)
00573       {
00574         std::string scene = s->text(0).toStdString();
00575         ROS_DEBUG("Attempting to load scene '%s'", scene.c_str());
00576         moveit_warehouse::PlanningSceneWithMetadata scene_m;
00577         bool got_ps = false;
00578         try
00579         {
00580           got_ps = planning_scene_storage_->getPlanningScene(scene_m, scene);
00581         }
00582         catch (std::runtime_error& ex)
00583         {
00584           ROS_ERROR("%s", ex.what());
00585         }
00586 
00587         if (got_ps)
00588         {
00589           ROS_INFO("Loaded scene '%s'", scene.c_str());
00590           if (planning_display_->getPlanningSceneMonitor())
00591           {
00592             if (scene_m->robot_model_name != planning_display_->getRobotModel()->getName())
00593             {
00594               ROS_INFO("Scene '%s' was saved for robot '%s' but we are using robot '%s'. Using scene geometry only",
00595                        scene.c_str(), scene_m->robot_model_name.c_str(),
00596                        planning_display_->getRobotModel()->getName().c_str());
00597               planning_scene_world_publisher_.publish(scene_m->world);
00598               // publish the parts that are not in the world
00599               moveit_msgs::PlanningScene diff;
00600               diff.is_diff = true;
00601               diff.name = scene_m->name;
00602               planning_scene_publisher_.publish(diff);
00603             }
00604             else
00605               planning_scene_publisher_.publish(static_cast<const moveit_msgs::PlanningScene&>(*scene_m));
00606           }
00607           else
00608             planning_scene_publisher_.publish(static_cast<const moveit_msgs::PlanningScene&>(*scene_m));
00609         }
00610         else
00611           ROS_WARN("Failed to load scene '%s'. Has the message format changed since the scene was saved?",
00612                    scene.c_str());
00613       }
00614     }
00615   }
00616 }
00617 
00618 void MotionPlanningFrame::computeLoadQueryButtonClicked()
00619 {
00620   if (planning_scene_storage_)
00621   {
00622     QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00623     if (!sel.empty())
00624     {
00625       QTreeWidgetItem* s = sel.front();
00626       if (s->type() == ITEM_TYPE_QUERY)
00627       {
00628         std::string scene = s->parent()->text(0).toStdString();
00629         std::string query_name = s->text(0).toStdString();
00630         moveit_warehouse::MotionPlanRequestWithMetadata mp;
00631         bool got_q = false;
00632         try
00633         {
00634           got_q = planning_scene_storage_->getPlanningQuery(mp, scene, query_name);
00635         }
00636         catch (std::runtime_error& ex)
00637         {
00638           ROS_ERROR("%s", ex.what());
00639         }
00640 
00641         if (got_q)
00642         {
00643           robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState()));
00644           robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(),
00645                                                  mp->start_state, *start_state);
00646           planning_display_->setQueryStartState(*start_state);
00647 
00648           robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState()));
00649           for (std::size_t i = 0; i < mp->goal_constraints.size(); ++i)
00650             if (mp->goal_constraints[i].joint_constraints.size() > 0)
00651             {
00652               std::map<std::string, double> vals;
00653               for (std::size_t j = 0; j < mp->goal_constraints[i].joint_constraints.size(); ++j)
00654                 vals[mp->goal_constraints[i].joint_constraints[j].joint_name] =
00655                     mp->goal_constraints[i].joint_constraints[j].position;
00656               goal_state->setVariablePositions(vals);
00657               break;
00658             }
00659           planning_display_->setQueryGoalState(*goal_state);
00660         }
00661         else
00662           ROS_ERROR("Failed to load planning query '%s'. Has the message format changed since the query was saved?",
00663                     query_name.c_str());
00664       }
00665     }
00666   }
00667 }
00668 
00669 void MotionPlanningFrame::addObject(const collision_detection::WorldPtr& world, const std::string& id,
00670                                     const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose)
00671 {
00672   world->addToObject(id, shape, pose);
00673 
00674   planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
00675 
00676   // Automatically select the inserted object so that its IM is displayed
00677   planning_display_->addMainLoopJob(
00678       boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, id, true, ui_->collision_objects_list));
00679 
00680   planning_display_->queueRenderSceneGeometry();
00681 }
00682 
00683 void MotionPlanningFrame::createSceneInteractiveMarker()
00684 {
00685   QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00686   if (sel.empty())
00687     return;
00688 
00689   const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00690   if (!ps)
00691     return;
00692 
00693   const collision_detection::CollisionWorld::ObjectConstPtr& obj =
00694       ps->getWorld()->getObject(sel[0]->text().toStdString());
00695   if (obj && obj->shapes_.size() == 1)
00696   {
00697     Eigen::Quaterniond eq(obj->shape_poses_[0].rotation());
00698     geometry_msgs::PoseStamped shape_pose;
00699     shape_pose.pose.position.x = obj->shape_poses_[0].translation()[0];
00700     shape_pose.pose.position.y = obj->shape_poses_[0].translation()[1];
00701     shape_pose.pose.position.z = obj->shape_poses_[0].translation()[2];
00702     shape_pose.pose.orientation.x = eq.x();
00703     shape_pose.pose.orientation.y = eq.y();
00704     shape_pose.pose.orientation.z = eq.z();
00705     shape_pose.pose.orientation.w = eq.w();
00706 
00707     // create an interactive marker for moving the shape in the world
00708     visualization_msgs::InteractiveMarker int_marker =
00709         robot_interaction::make6DOFMarker(std::string("marker_") + sel[0]->text().toStdString(), shape_pose, 1.0);
00710     int_marker.header.frame_id = planning_display_->getRobotModel()->getModelFrame();
00711     int_marker.description = sel[0]->text().toStdString();
00712 
00713     rviz::InteractiveMarker* imarker = new rviz::InteractiveMarker(planning_display_->getSceneNode(), context_);
00714     interactive_markers::autoComplete(int_marker);
00715     imarker->processMessage(int_marker);
00716     imarker->setShowAxes(false);
00717     scene_marker_.reset(imarker);
00718 
00719     // Connect signals
00720     connect(imarker, SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this,
00721             SLOT(imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00722   }
00723   else
00724   {
00725     scene_marker_.reset();
00726   }
00727 }
00728 
00729 void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item)
00730 {
00731   long unsigned int version = known_collision_objects_version_;
00732   if (item->text().isEmpty())
00733   {
00734     QMessageBox::warning(this, "Invalid object name", "Cannot set an empty object name.");
00735     if (version == known_collision_objects_version_)
00736       item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
00737     return;
00738   }
00739 
00740   std::string item_text = item->text().toStdString();
00741   bool already_exists = planning_display_->getPlanningSceneRO()->getWorld()->hasObject(item_text);
00742   if (!already_exists)
00743     already_exists = planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(item_text);
00744   if (already_exists)
00745   {
00746     QMessageBox::warning(this, "Duplicate object name",
00747                          QString("The name '")
00748                              .append(item->text())
00749                              .append("' already exists. Not renaming object ")
00750                              .append((known_collision_objects_[item->type()].first.c_str())));
00751     if (version == known_collision_objects_version_)
00752       item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
00753     return;
00754   }
00755 
00756   if (item->checkState() == Qt::Unchecked)
00757   {
00758     planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00759     collision_detection::CollisionWorld::ObjectConstPtr obj =
00760         ps->getWorld()->getObject(known_collision_objects_[item->type()].first);
00761     if (obj)
00762     {
00763       known_collision_objects_[item->type()].first = item_text;
00764       ps->getWorldNonConst()->removeObject(obj->id_);
00765       ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_,
00766                                           obj->shape_poses_);
00767       if (scene_marker_)
00768       {
00769         scene_marker_.reset();
00770         planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this));
00771       }
00772     }
00773   }
00774   else
00775   {
00776     // rename attached body
00777     planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00778     robot_state::RobotState& cs = ps->getCurrentStateNonConst();
00779     const robot_state::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first);
00780     if (ab)
00781     {
00782       known_collision_objects_[item->type()].first = item_text;
00783       robot_state::AttachedBody* new_ab = new robot_state::AttachedBody(
00784           ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getShapes(),
00785           ab->getFixedTransforms(), ab->getTouchLinks(), ab->getDetachPosture());
00786       cs.clearAttachedBody(ab->getName());
00787       cs.attachBody(new_ab);
00788     }
00789   }
00790 }
00791 
00792 void MotionPlanningFrame::attachDetachCollisionObject(QListWidgetItem* item)
00793 {
00794   long unsigned int version = known_collision_objects_version_;
00795   bool checked = item->checkState() == Qt::Checked;
00796   std::pair<std::string, bool> data = known_collision_objects_[item->type()];
00797   moveit_msgs::AttachedCollisionObject aco;
00798 
00799   if (checked)  // we need to attach a known collision object
00800   {
00801     QStringList links;
00802     const std::vector<std::string>& links_std = planning_display_->getRobotModel()->getLinkModelNames();
00803     for (std::size_t i = 0; i < links_std.size(); ++i)
00804       links.append(QString::fromStdString(links_std[i]));
00805     bool ok = false;
00806     QString response =
00807         QInputDialog::getItem(this, tr("Select Link Name"), tr("Choose the link to attach to:"), links, 0, false, &ok);
00808     if (!ok)
00809     {
00810       if (version == known_collision_objects_version_)
00811         item->setCheckState(Qt::Unchecked);
00812       return;
00813     }
00814     aco.link_name = response.toStdString();
00815     aco.object.id = data.first;
00816     aco.object.operation = moveit_msgs::CollisionObject::ADD;
00817   }
00818   else  // we need to detach an attached object
00819   {
00820     const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00821     const robot_state::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first);
00822     if (attached_body)
00823     {
00824       aco.link_name = attached_body->getAttachedLinkName();
00825       aco.object.id = attached_body->getName();
00826       aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00827     }
00828   }
00829   {
00830     planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00831     // we loop through the list in case updates were received since the start of the function
00832     for (std::size_t i = 0; i < known_collision_objects_.size(); ++i)
00833       if (known_collision_objects_[i].first == data.first)
00834       {
00835         known_collision_objects_[i].second = checked;
00836         break;
00837       }
00838     ps->processAttachedCollisionObjectMsg(aco);
00839   }
00840 
00841   selectedCollisionObjectChanged();
00842   planning_display_->queueRenderSceneGeometry();
00843 }
00844 
00845 void MotionPlanningFrame::populateCollisionObjectsList()
00846 {
00847   ui_->collision_objects_list->setUpdatesEnabled(false);
00848   bool oldState = ui_->collision_objects_list->blockSignals(true);
00849 
00850   {
00851     QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
00852     std::set<std::string> to_select;
00853     for (int i = 0; i < sel.size(); ++i)
00854       to_select.insert(sel[i]->text().toStdString());
00855     ui_->collision_objects_list->clear();
00856     known_collision_objects_.clear();
00857     known_collision_objects_version_++;
00858 
00859     planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
00860     if (ps)
00861     {
00862       const std::vector<std::string>& collision_object_names = ps->getWorld()->getObjectIds();
00863       for (std::size_t i = 0; i < collision_object_names.size(); ++i)
00864       {
00865         if (collision_object_names[i] == planning_scene::PlanningScene::OCTOMAP_NS)
00866           continue;
00867 
00868         QListWidgetItem* item =
00869             new QListWidgetItem(QString::fromStdString(collision_object_names[i]), ui_->collision_objects_list, (int)i);
00870         item->setFlags(item->flags() | Qt::ItemIsEditable);
00871         item->setToolTip(item->text());
00872         item->setCheckState(Qt::Unchecked);
00873         if (to_select.find(collision_object_names[i]) != to_select.end())
00874           item->setSelected(true);
00875         ui_->collision_objects_list->addItem(item);
00876         known_collision_objects_.push_back(std::make_pair(collision_object_names[i], false));
00877       }
00878 
00879       const robot_state::RobotState& cs = ps->getCurrentState();
00880       std::vector<const robot_state::AttachedBody*> attached_bodies;
00881       cs.getAttachedBodies(attached_bodies);
00882       for (std::size_t i = 0; i < attached_bodies.size(); ++i)
00883       {
00884         QListWidgetItem* item =
00885             new QListWidgetItem(QString::fromStdString(attached_bodies[i]->getName()), ui_->collision_objects_list,
00886                                 (int)(i + collision_object_names.size()));
00887         item->setFlags(item->flags() | Qt::ItemIsEditable);
00888         item->setToolTip(item->text());
00889         item->setCheckState(Qt::Checked);
00890         if (to_select.find(attached_bodies[i]->getName()) != to_select.end())
00891           item->setSelected(true);
00892         ui_->collision_objects_list->addItem(item);
00893         known_collision_objects_.push_back(std::make_pair(attached_bodies[i]->getName(), true));
00894       }
00895     }
00896   }
00897 
00898   ui_->collision_objects_list->blockSignals(oldState);
00899   ui_->collision_objects_list->setUpdatesEnabled(true);
00900   selectedCollisionObjectChanged();
00901 }
00902 
00903 void MotionPlanningFrame::exportAsTextButtonClicked()
00904 {
00905   QString path =
00906       QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
00907   if (!path.isEmpty())
00908     planning_display_->addBackgroundJob(
00909         boost::bind(&MotionPlanningFrame::computeExportAsText, this, path.toStdString()), "export as text");
00910 }
00911 
00912 void MotionPlanningFrame::computeExportAsText(const std::string& path)
00913 {
00914   planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
00915   if (ps)
00916   {
00917     std::string p = (path.length() < 7 || path.substr(path.length() - 6) != ".scene") ? path + ".scene" : path;
00918     std::ofstream fout(p.c_str());
00919     if (fout.good())
00920     {
00921       ps->saveGeometryToStream(fout);
00922       fout.close();
00923       ROS_INFO("Saved current scene geometry to '%s'", p.c_str());
00924     }
00925     else
00926       ROS_WARN("Unable to save current scene geometry to '%s'", p.c_str());
00927   }
00928 }
00929 
00930 void MotionPlanningFrame::computeImportFromText(const std::string& path)
00931 {
00932   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00933   if (ps)
00934   {
00935     std::ifstream fin(path.c_str());
00936     if (fin.good())
00937     {
00938       ps->loadGeometryFromStream(fin);
00939       fin.close();
00940       ROS_INFO("Loaded scene geometry from '%s'", path.c_str());
00941       planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
00942       planning_display_->queueRenderSceneGeometry();
00943     }
00944     else
00945       ROS_WARN("Unable to load scene geometry from '%s'", path.c_str());
00946   }
00947 }
00948 
00949 void MotionPlanningFrame::importFromTextButtonClicked()
00950 {
00951   QString path =
00952       QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
00953   if (!path.isEmpty())
00954     planning_display_->addBackgroundJob(
00955         boost::bind(&MotionPlanningFrame::computeImportFromText, this, path.toStdString()), "import from text");
00956 }
00957 }


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:25:00