motion_planning_frame_manipulation.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
39 
42 #include <object_recognition_msgs/ObjectRecognitionGoal.h>
43 
44 #include "ui_motion_planning_rviz_plugin_frame.h"
45 
46 namespace moveit_rviz_plugin
47 {
50 {
51  if (!semantic_world_)
52  {
54  if (ps)
55  {
57  }
58  if (semantic_world_)
59  {
60  semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this));
61  }
62  }
64  "detect objects");
65 }
66 
68 {
69  pick_object_name_.clear();
70 
71  std::vector<std::string> objects, object_ids;
72  double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
73  double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
74  double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
75 
76  double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
77  double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
78  double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
79 
80  ros::Time start_time = ros::Time::now();
81  while (object_ids.empty() && ros::Time::now() - start_time <= ros::Duration(3.0))
82  {
83  object_ids =
84  planning_scene_interface_->getKnownObjectNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z, true, objects);
85  ros::Duration(0.5).sleep();
86  }
87 
88  ROS_DEBUG("Found %d objects", (int)object_ids.size());
89  updateDetectedObjectsList(object_ids, objects);
90 }
91 
93 {
94  QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
95  if (sel.empty())
96  {
97  ROS_INFO("No objects to select");
98  return;
99  }
101  std_msgs::ColorRGBA pick_object_color;
102  pick_object_color.r = 1.0;
103  pick_object_color.g = 0.0;
104  pick_object_color.b = 0.0;
105  pick_object_color.a = 1.0;
106 
107  if (ps)
108  {
109  if (!selected_object_name_.empty())
110  ps->removeObjectColor(selected_object_name_);
111  selected_object_name_ = sel[0]->text().toStdString();
112  ps->setObjectColor(selected_object_name_, pick_object_color);
113  }
114 }
115 
117 {
118 }
119 
121 {
123  {
126  false));
127  try
128  {
130  }
131  catch (std::exception& ex)
132  {
133  ROS_ERROR("Object recognition action: %s", ex.what());
134  return;
135  }
136  }
137  object_recognition_msgs::ObjectRecognitionGoal goal;
138  object_recognition_client_->sendGoal(goal);
139  if (!object_recognition_client_->waitForResult())
140  {
141  ROS_INFO_STREAM("Object recognition client returned early");
142  }
144  {
145  ROS_WARN_STREAM("Fail: " << object_recognition_client_->getState().toString() << ": "
146  << object_recognition_client_->getState().getText());
147  }
148 }
149 
150 void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& msg)
151 {
152  ros::Duration(1.0).sleep();
154 }
155 
156 void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::string>& object_ids,
157  const std::vector<std::string>& objects)
158 {
159  ui_->detected_objects_list->setUpdatesEnabled(false);
160  bool oldState = ui_->detected_objects_list->blockSignals(true);
161  ui_->detected_objects_list->clear();
162  {
163  for (std::size_t i = 0; i < object_ids.size(); ++i)
164  {
165  QListWidgetItem* item =
166  new QListWidgetItem(QString::fromStdString(object_ids[i]), ui_->detected_objects_list, (int)i);
167  item->setToolTip(item->text());
168  Qt::ItemFlags flags = item->flags();
169  flags &= ~(Qt::ItemIsUserCheckable);
170  item->setFlags(flags);
171  ui_->detected_objects_list->addItem(item);
172  }
173  }
174  ui_->detected_objects_list->blockSignals(oldState);
175  ui_->detected_objects_list->setUpdatesEnabled(true);
176  if (!object_ids.empty())
177  ui_->pick_button->setEnabled(true);
178 }
179 
182 {
183  ROS_DEBUG("Update table callback");
184  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::publishTables, this), "publish tables");
185 }
186 
188 {
189  semantic_world_->addTablesToCollisionWorld();
191 }
192 
194 {
195  QList<QListWidgetItem*> sel = ui_->support_surfaces_list->selectedItems();
196  if (sel.empty())
197  {
198  ROS_INFO("No tables to select");
199  return;
200  }
202  std_msgs::ColorRGBA selected_support_surface_color;
203  selected_support_surface_color.r = 0.0;
204  selected_support_surface_color.g = 0.0;
205  selected_support_surface_color.b = 1.0;
206  selected_support_surface_color.a = 1.0;
207 
208  if (ps)
209  {
210  if (!selected_support_surface_name_.empty())
211  ps->removeObjectColor(selected_support_surface_name_);
212  selected_support_surface_name_ = sel[0]->text().toStdString();
213  ps->setObjectColor(selected_support_surface_name_, selected_support_surface_color);
214  }
215 }
216 
218 {
219  double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
220  double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
221  double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
222 
223  double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
224  double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
225  double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
226  std::vector<std::string> support_ids = semantic_world_->getTableNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z);
227  ROS_INFO("%d Tables in collision world", (int)support_ids.size());
228 
229  ui_->support_surfaces_list->setUpdatesEnabled(false);
230  bool oldState = ui_->support_surfaces_list->blockSignals(true);
231  ui_->support_surfaces_list->clear();
232  {
233  for (std::size_t i = 0; i < support_ids.size(); ++i)
234  {
235  QListWidgetItem* item =
236  new QListWidgetItem(QString::fromStdString(support_ids[i]), ui_->support_surfaces_list, (int)i);
237  item->setToolTip(item->text());
238  Qt::ItemFlags flags = item->flags();
239  flags &= ~(Qt::ItemIsUserCheckable);
240  item->setFlags(flags);
241  ui_->support_surfaces_list->addItem(item);
242  }
243  }
244  ui_->support_surfaces_list->blockSignals(oldState);
245  ui_->support_surfaces_list->setUpdatesEnabled(true);
246 }
247 
250 {
251  QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
252  QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
253 
254  std::string group_name = planning_display_->getCurrentPlanningGroup();
255  if (sel.empty())
256  {
257  ROS_INFO("No objects to pick");
258  return;
259  }
260  pick_object_name_[group_name] = sel[0]->text().toStdString();
261 
262  if (!sel_table.empty())
263  support_surface_name_ = sel_table[0]->text().toStdString();
264  else
265  {
266  if (semantic_world_)
267  {
268  std::vector<std::string> object_names;
269  object_names.push_back(pick_object_name_[group_name]);
270  std::map<std::string, geometry_msgs::Pose> object_poses = planning_scene_interface_->getObjectPoses(object_names);
271  if (object_poses.find(pick_object_name_[group_name]) != object_poses.end())
272  {
273  ROS_DEBUG("Finding current table for object: %s", pick_object_name_[group_name].c_str());
274  support_surface_name_ = semantic_world_->findObjectTable(object_poses[pick_object_name_[group_name]]);
275  }
276  else
277  support_surface_name_.clear();
278  }
279  else
280  support_surface_name_.clear();
281  }
282  ROS_INFO("Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(),
283  support_surface_name_.c_str());
285 }
286 
288 {
289  QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
290  std::string group_name = planning_display_->getCurrentPlanningGroup();
291 
292  if (!sel_table.empty())
293  support_surface_name_ = sel_table[0]->text().toStdString();
294  else
295  {
296  support_surface_name_.clear();
297  ROS_ERROR("Need to specify table to place object on");
298  return;
299  }
300 
301  ui_->pick_button->setEnabled(false);
302  ui_->place_button->setEnabled(false);
303 
304  std::vector<const robot_state::AttachedBody*> attached_bodies;
306  if (!ps)
307  {
308  ROS_ERROR("No planning scene");
309  return;
310  }
311  const robot_model::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name);
312  if (jmg)
313  ps->getCurrentState().getAttachedBodies(attached_bodies, jmg);
314 
315  if (attached_bodies.empty())
316  {
317  ROS_ERROR("No bodies to place");
318  return;
319  }
320 
321  geometry_msgs::Quaternion upright_orientation;
322  upright_orientation.w = 1.0;
323 
324  // Else place the first one
325  place_poses_.clear();
326  place_poses_ = semantic_world_->generatePlacePoses(support_surface_name_, attached_bodies[0]->getShapes()[0],
327  upright_orientation, 0.1);
329  place_object_name_ = attached_bodies[0]->getName();
331 }
332 
334 {
335  std::string group_name = planning_display_->getCurrentPlanningGroup();
336  ui_->pick_button->setEnabled(false);
337  if (pick_object_name_.find(group_name) == pick_object_name_.end())
338  {
339  ROS_ERROR("No pick object set for this group");
340  return;
341  }
342  if (!support_surface_name_.empty())
343  {
344  move_group_->setSupportSurfaceName(support_surface_name_);
345  }
346  if (move_group_->pick(pick_object_name_[group_name]))
347  {
348  ui_->place_button->setEnabled(true);
349  }
350 }
351 
353 {
355  return;
356 }
357 }
void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr &msg)
void updateDetectedObjectsList(const std::vector< std::string > &object_ids, const std::vector< std::string > &objects)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void waitForAction(const T &action, const ros::NodeHandle &node_handle, const ros::Duration &wait_for_server, const std::string &name)
bool sleep() const
text
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
void visualizePlaceLocations(const std::vector< geometry_msgs::PoseStamped > &place_poses)
#define ROS_INFO(...)
#define ROS_WARN_STREAM(args)
const std::string OBJECT_RECOGNITION_ACTION
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
#define ROS_INFO_STREAM(args)
static Time now()
std::vector< geometry_msgs::PoseStamped > place_poses_
std::map< std::string, std::string > pick_object_name_
#define ROS_ERROR(...)
std::unique_ptr< actionlib::SimpleActionClient< object_recognition_msgs::ObjectRecognitionAction > > object_recognition_client_
moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
moveit::semantic_world::SemanticWorldPtr semantic_world_
#define ROS_DEBUG(...)


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:08