motion_planning_frame_manipulation.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: Sachin Chitta */
00036 
00037 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00038 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
00039 
00040 #include <moveit/kinematic_constraints/utils.h>
00041 #include <moveit/robot_state/conversions.h>
00042 #include <object_recognition_msgs/ObjectRecognitionGoal.h>
00043 
00044 #include "ui_motion_planning_rviz_plugin_frame.h"
00045 
00046 namespace moveit_rviz_plugin
00047 {
00049 void MotionPlanningFrame::detectObjectsButtonClicked()
00050 {
00051   if (!semantic_world_)
00052   {
00053     const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00054     if (ps)
00055     {
00056       semantic_world_.reset(new moveit::semantic_world::SemanticWorld(ps));
00057     }
00058     if (semantic_world_)
00059     {
00060       semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this));
00061     }
00062   }
00063   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::triggerObjectDetection, this), "detect "
00064                                                                                                        "objects");
00065 }
00066 
00067 void MotionPlanningFrame::processDetectedObjects()
00068 {
00069   pick_object_name_.clear();
00070 
00071   std::vector<std::string> objects, object_ids;
00072   double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
00073   double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
00074   double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
00075 
00076   double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
00077   double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
00078   double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
00079 
00080   ros::Time start_time = ros::Time::now();
00081   while (object_ids.empty() && ros::Time::now() - start_time <= ros::Duration(3.0))
00082   {
00083     object_ids =
00084         planning_scene_interface_->getKnownObjectNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z, true, objects);
00085     ros::Duration(0.5).sleep();
00086   }
00087 
00088   ROS_DEBUG("Found %d objects", (int)object_ids.size());
00089   updateDetectedObjectsList(object_ids, objects);
00090 }
00091 
00092 void MotionPlanningFrame::selectedDetectedObjectChanged()
00093 {
00094   QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
00095   if (sel.empty())
00096   {
00097     ROS_INFO("No objects to select");
00098     return;
00099   }
00100   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00101   std_msgs::ColorRGBA pick_object_color;
00102   pick_object_color.r = 1.0;
00103   pick_object_color.g = 0.0;
00104   pick_object_color.b = 0.0;
00105   pick_object_color.a = 1.0;
00106 
00107   if (ps)
00108   {
00109     if (!selected_object_name_.empty())
00110       ps->removeObjectColor(selected_object_name_);
00111     selected_object_name_ = sel[0]->text().toStdString();
00112     ps->setObjectColor(selected_object_name_, pick_object_color);
00113   }
00114 }
00115 
00116 void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* item)
00117 {
00118 }
00119 
00120 void MotionPlanningFrame::triggerObjectDetection()
00121 {
00122   if (!object_recognition_client_)
00123   {
00124     object_recognition_client_.reset(
00125         new actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction>(OBJECT_RECOGNITION_ACTION,
00126                                                                                             false));
00127     try
00128     {
00129       waitForAction(object_recognition_client_, nh_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION);
00130     }
00131     catch (std::runtime_error& ex)
00132     {
00133       ROS_ERROR("Object recognition action: %s", ex.what());
00134       return;
00135     }
00136   }
00137   object_recognition_msgs::ObjectRecognitionGoal goal;
00138   object_recognition_client_->sendGoal(goal);
00139   if (!object_recognition_client_->waitForResult())
00140   {
00141     ROS_INFO_STREAM("Object recognition client returned early");
00142   }
00143   if (object_recognition_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00144   {
00145     ROS_WARN_STREAM("Fail: " << object_recognition_client_->getState().toString() << ": "
00146                              << object_recognition_client_->getState().getText());
00147   }
00148 }
00149 
00150 void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg)
00151 {
00152   ros::Duration(1.0).sleep();
00153   planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this));
00154 }
00155 
00156 void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::string>& object_ids,
00157                                                     const std::vector<std::string>& objects)
00158 {
00159   ui_->detected_objects_list->setUpdatesEnabled(false);
00160   bool oldState = ui_->detected_objects_list->blockSignals(true);
00161   ui_->detected_objects_list->clear();
00162   {
00163     for (std::size_t i = 0; i < object_ids.size(); ++i)
00164     {
00165       QListWidgetItem* item =
00166           new QListWidgetItem(QString::fromStdString(object_ids[i]), ui_->detected_objects_list, (int)i);
00167       item->setToolTip(item->text());
00168       Qt::ItemFlags flags = item->flags();
00169       flags &= ~(Qt::ItemIsUserCheckable);
00170       item->setFlags(flags);
00171       ui_->detected_objects_list->addItem(item);
00172     }
00173   }
00174   ui_->detected_objects_list->blockSignals(oldState);
00175   ui_->detected_objects_list->setUpdatesEnabled(true);
00176   if (!object_ids.empty())
00177     ui_->pick_button->setEnabled(true);
00178 }
00179 
00181 void MotionPlanningFrame::updateTables()
00182 {
00183   ROS_DEBUG("Update table callback");
00184   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::publishTables, this), "publish tables");
00185 }
00186 
00187 void MotionPlanningFrame::publishTables()
00188 {
00189   semantic_world_->addTablesToCollisionWorld();
00190   planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::updateSupportSurfacesList, this));
00191 }
00192 
00193 void MotionPlanningFrame::selectedSupportSurfaceChanged()
00194 {
00195   QList<QListWidgetItem*> sel = ui_->support_surfaces_list->selectedItems();
00196   if (sel.empty())
00197   {
00198     ROS_INFO("No tables to select");
00199     return;
00200   }
00201   planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
00202   std_msgs::ColorRGBA selected_support_surface_color;
00203   selected_support_surface_color.r = 0.0;
00204   selected_support_surface_color.g = 0.0;
00205   selected_support_surface_color.b = 1.0;
00206   selected_support_surface_color.a = 1.0;
00207 
00208   if (ps)
00209   {
00210     if (!selected_support_surface_name_.empty())
00211       ps->removeObjectColor(selected_support_surface_name_);
00212     selected_support_surface_name_ = sel[0]->text().toStdString();
00213     ps->setObjectColor(selected_support_surface_name_, selected_support_surface_color);
00214   }
00215 }
00216 
00217 void MotionPlanningFrame::updateSupportSurfacesList()
00218 {
00219   double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
00220   double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
00221   double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
00222 
00223   double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
00224   double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
00225   double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
00226   std::vector<std::string> support_ids = semantic_world_->getTableNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z);
00227   ROS_INFO("%d Tables in collision world", (int)support_ids.size());
00228 
00229   ui_->support_surfaces_list->setUpdatesEnabled(false);
00230   bool oldState = ui_->support_surfaces_list->blockSignals(true);
00231   ui_->support_surfaces_list->clear();
00232   {
00233     for (std::size_t i = 0; i < support_ids.size(); ++i)
00234     {
00235       QListWidgetItem* item =
00236           new QListWidgetItem(QString::fromStdString(support_ids[i]), ui_->support_surfaces_list, (int)i);
00237       item->setToolTip(item->text());
00238       Qt::ItemFlags flags = item->flags();
00239       flags &= ~(Qt::ItemIsUserCheckable);
00240       item->setFlags(flags);
00241       ui_->support_surfaces_list->addItem(item);
00242     }
00243   }
00244   ui_->support_surfaces_list->blockSignals(oldState);
00245   ui_->support_surfaces_list->setUpdatesEnabled(true);
00246 }
00247 
00249 void MotionPlanningFrame::pickObjectButtonClicked()
00250 {
00251   QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
00252   QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
00253 
00254   std::string group_name = planning_display_->getCurrentPlanningGroup();
00255   if (sel.empty())
00256   {
00257     ROS_INFO("No objects to pick");
00258     return;
00259   }
00260   pick_object_name_[group_name] = sel[0]->text().toStdString();
00261 
00262   if (!sel_table.empty())
00263     support_surface_name_ = sel_table[0]->text().toStdString();
00264   else
00265   {
00266     if (semantic_world_)
00267     {
00268       std::vector<std::string> object_names;
00269       object_names.push_back(pick_object_name_[group_name]);
00270       std::map<std::string, geometry_msgs::Pose> object_poses = planning_scene_interface_->getObjectPoses(object_names);
00271       if (object_poses.find(pick_object_name_[group_name]) != object_poses.end())
00272       {
00273         ROS_DEBUG("Finding current table for object: %s", pick_object_name_[group_name].c_str());
00274         support_surface_name_ = semantic_world_->findObjectTable(object_poses[pick_object_name_[group_name]]);
00275       }
00276       else
00277         support_surface_name_.clear();
00278     }
00279     else
00280       support_surface_name_.clear();
00281   }
00282   ROS_INFO("Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(),
00283            support_surface_name_.c_str());
00284   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::pickObject, this), "pick");
00285 }
00286 
00287 void MotionPlanningFrame::placeObjectButtonClicked()
00288 {
00289   QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
00290   std::string group_name = planning_display_->getCurrentPlanningGroup();
00291 
00292   if (!sel_table.empty())
00293     support_surface_name_ = sel_table[0]->text().toStdString();
00294   else
00295   {
00296     support_surface_name_.clear();
00297     ROS_ERROR("Need to specify table to place object on");
00298     return;
00299   }
00300 
00301   ui_->pick_button->setEnabled(false);
00302   ui_->place_button->setEnabled(false);
00303 
00304   std::vector<const robot_state::AttachedBody*> attached_bodies;
00305   const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
00306   if (!ps)
00307   {
00308     ROS_ERROR("No planning scene");
00309     return;
00310   }
00311   const robot_model::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name);
00312   if (jmg)
00313     ps->getCurrentState().getAttachedBodies(attached_bodies, jmg);
00314 
00315   if (attached_bodies.empty())
00316   {
00317     ROS_ERROR("No bodies to place");
00318     return;
00319   }
00320 
00321   geometry_msgs::Quaternion upright_orientation;
00322   upright_orientation.w = 1.0;
00323 
00324   // Else place the first one
00325   place_poses_.clear();
00326   place_poses_ = semantic_world_->generatePlacePoses(support_surface_name_, attached_bodies[0]->getShapes()[0],
00327                                                      upright_orientation, 0.1);
00328   planning_display_->visualizePlaceLocations(place_poses_);
00329   place_object_name_ = attached_bodies[0]->getName();
00330   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::placeObject, this), "place");
00331 }
00332 
00333 void MotionPlanningFrame::pickObject()
00334 {
00335   std::string group_name = planning_display_->getCurrentPlanningGroup();
00336   ui_->pick_button->setEnabled(false);
00337   if (pick_object_name_.find(group_name) == pick_object_name_.end())
00338   {
00339     ROS_ERROR("No pick object set for this group");
00340     return;
00341   }
00342   if (!support_surface_name_.empty())
00343   {
00344     move_group_->setSupportSurfaceName(support_surface_name_);
00345   }
00346   if (move_group_->pick(pick_object_name_[group_name]))
00347   {
00348     ui_->place_button->setEnabled(true);
00349   }
00350 }
00351 
00352 void MotionPlanningFrame::placeObject()
00353 {
00354   move_group_->place(place_object_name_, place_poses_);
00355   return;
00356 }
00357 }


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:13