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 <tf2_eigen/tf2_eigen.h>
45 
46 #include "ui_motion_planning_rviz_plugin_frame.h"
47 
48 namespace moveit_rviz_plugin
49 {
52 {
53  if (!semantic_world_)
54  {
56  if (ps)
57  {
58  semantic_world_ = std::make_shared<moveit::semantic_world::SemanticWorld>(ps);
59  }
60  if (semantic_world_)
61  {
62  semantic_world_->addTableCallback([this] { updateTables(); });
63  }
64  }
65  planning_display_->addBackgroundJob([this] { triggerObjectDetection(); }, "detect objects");
66 }
67 
69 {
70  pick_object_name_.clear();
71 
72  std::vector<std::string> object_ids;
73  double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
74  double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
75  double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
76 
77  double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
78  double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
79  double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
80 
81  ros::Time start_time = ros::Time::now();
82  while (object_ids.empty() && ros::Time::now() - start_time <= ros::Duration(3.0))
83  {
84  // collect all objects in region of interest
85  {
87  const collision_detection::WorldConstPtr& world = ps->getWorld();
88  object_ids.clear();
89  for (const auto& object : *world)
90  {
91  const auto& position = object.second->pose_.translation();
92  if (position.x() >= min_x && position.x() <= max_x && position.y() >= min_y && position.y() <= max_y &&
93  position.z() >= min_z && position.z() <= max_z)
94  {
95  object_ids.push_back(object.first);
96  }
97  }
98  if (!object_ids.empty())
99  break;
100  }
101  ros::Duration(0.5).sleep();
102  }
103 
104  ROS_DEBUG("Found %d objects", (int)object_ids.size());
105  updateDetectedObjectsList(object_ids);
106 }
107 
109 {
110  QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
111  if (sel.empty())
112  {
113  ROS_INFO("No objects to select");
114  return;
115  }
117  std_msgs::ColorRGBA pick_object_color;
118  pick_object_color.r = 1.0;
119  pick_object_color.g = 0.0;
120  pick_object_color.b = 0.0;
121  pick_object_color.a = 1.0;
122 
123  if (ps)
124  {
125  if (!selected_object_name_.empty())
126  ps->removeObjectColor(selected_object_name_);
127  selected_object_name_ = sel[0]->text().toStdString();
128  ps->setObjectColor(selected_object_name_, pick_object_color);
129  }
130 }
131 
132 void MotionPlanningFrame::detectedObjectChanged(QListWidgetItem* /*item*/)
133 {
134 }
135 
137 {
139  {
141  std::make_unique<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction>>(
143  try
144  {
146  }
147  catch (std::exception& ex)
148  {
149  ROS_ERROR("Object recognition action: %s", ex.what());
150  return;
151  }
152  }
153  object_recognition_msgs::ObjectRecognitionGoal goal;
154  object_recognition_client_->sendGoal(goal);
155  if (!object_recognition_client_->waitForResult())
156  {
157  ROS_INFO_STREAM("Object recognition client returned early");
158  }
160  {
161  ROS_WARN_STREAM("Fail: " << object_recognition_client_->getState().toString() << ": "
162  << object_recognition_client_->getState().getText());
163  }
164 }
165 
166 void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& /*msg*/)
167 {
170 }
171 
172 void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::string>& object_ids)
173 {
174  ui_->detected_objects_list->setUpdatesEnabled(false);
175  bool old_state = ui_->detected_objects_list->blockSignals(true);
176  ui_->detected_objects_list->clear();
177  {
178  for (std::size_t i = 0; i < object_ids.size(); ++i)
179  {
180  QListWidgetItem* item =
181  new QListWidgetItem(QString::fromStdString(object_ids[i]), ui_->detected_objects_list, (int)i);
182  item->setToolTip(item->text());
183  Qt::ItemFlags flags = item->flags();
184  flags &= ~(Qt::ItemIsUserCheckable);
185  item->setFlags(flags);
186  ui_->detected_objects_list->addItem(item);
187  }
188  }
189  ui_->detected_objects_list->blockSignals(old_state);
190  ui_->detected_objects_list->setUpdatesEnabled(true);
191  if (!object_ids.empty())
192  ui_->pick_button->setEnabled(true);
193 }
194 
197 {
198  ROS_DEBUG("Update table callback");
199  planning_display_->addBackgroundJob([this] { publishTables(); }, "publish tables");
200 }
201 
203 {
204  semantic_world_->addTablesToCollisionWorld();
206 }
207 
209 {
210  QList<QListWidgetItem*> sel = ui_->support_surfaces_list->selectedItems();
211  if (sel.empty())
212  {
213  ROS_INFO("No tables to select");
214  return;
215  }
217  std_msgs::ColorRGBA selected_support_surface_color;
218  selected_support_surface_color.r = 0.0;
219  selected_support_surface_color.g = 0.0;
220  selected_support_surface_color.b = 1.0;
221  selected_support_surface_color.a = 1.0;
222 
223  if (ps)
224  {
225  if (!selected_support_surface_name_.empty())
226  ps->removeObjectColor(selected_support_surface_name_);
227  selected_support_surface_name_ = sel[0]->text().toStdString();
228  ps->setObjectColor(selected_support_surface_name_, selected_support_surface_color);
229  }
230 }
231 
233 {
234  double min_x = ui_->roi_center_x->value() - ui_->roi_size_x->value() / 2.0;
235  double min_y = ui_->roi_center_y->value() - ui_->roi_size_y->value() / 2.0;
236  double min_z = ui_->roi_center_z->value() - ui_->roi_size_z->value() / 2.0;
237 
238  double max_x = ui_->roi_center_x->value() + ui_->roi_size_x->value() / 2.0;
239  double max_y = ui_->roi_center_y->value() + ui_->roi_size_y->value() / 2.0;
240  double max_z = ui_->roi_center_z->value() + ui_->roi_size_z->value() / 2.0;
241  std::vector<std::string> support_ids = semantic_world_->getTableNamesInROI(min_x, min_y, min_z, max_x, max_y, max_z);
242  ROS_INFO("%d Tables in collision world", (int)support_ids.size());
243 
244  ui_->support_surfaces_list->setUpdatesEnabled(false);
245  bool old_state = ui_->support_surfaces_list->blockSignals(true);
246  ui_->support_surfaces_list->clear();
247  {
248  for (std::size_t i = 0; i < support_ids.size(); ++i)
249  {
250  QListWidgetItem* item =
251  new QListWidgetItem(QString::fromStdString(support_ids[i]), ui_->support_surfaces_list, (int)i);
252  item->setToolTip(item->text());
253  Qt::ItemFlags flags = item->flags();
254  flags &= ~(Qt::ItemIsUserCheckable);
255  item->setFlags(flags);
256  ui_->support_surfaces_list->addItem(item);
257  }
258  }
259  ui_->support_surfaces_list->blockSignals(old_state);
260  ui_->support_surfaces_list->setUpdatesEnabled(true);
261 }
262 
265 {
266  QList<QListWidgetItem*> sel = ui_->detected_objects_list->selectedItems();
267  QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
268 
269  std::string group_name = planning_display_->getCurrentPlanningGroup();
270  if (sel.empty())
271  {
272  ROS_INFO("No objects to pick");
273  return;
274  }
275  pick_object_name_[group_name] = sel[0]->text().toStdString();
276 
277  if (!sel_table.empty())
278  support_surface_name_ = sel_table[0]->text().toStdString();
279  else
280  {
281  if (semantic_world_)
282  {
284  if (ps->getWorld()->hasObject(pick_object_name_[group_name]))
285  {
286  geometry_msgs::Pose object_pose(tf2::toMsg(ps->getWorld()->getTransform(pick_object_name_[group_name])));
287  ROS_DEBUG_STREAM("Finding current table for object: " << pick_object_name_[group_name]);
288  support_surface_name_ = semantic_world_->findObjectTable(object_pose);
289  }
290  else
291  support_surface_name_.clear();
292  }
293  else
294  support_surface_name_.clear();
295  }
296  ROS_INFO("Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(),
297  support_surface_name_.c_str());
298  planning_display_->addBackgroundJob([this] { pickObject(); }, "pick");
299 }
300 
302 {
303  QList<QListWidgetItem*> sel_table = ui_->support_surfaces_list->selectedItems();
304  std::string group_name = planning_display_->getCurrentPlanningGroup();
305 
306  if (!sel_table.empty())
307  support_surface_name_ = sel_table[0]->text().toStdString();
308  else
309  {
310  support_surface_name_.clear();
311  ROS_ERROR("Need to specify table to place object on");
312  return;
313  }
314 
315  ui_->pick_button->setEnabled(false);
316  ui_->place_button->setEnabled(false);
317 
318  std::vector<const moveit::core::AttachedBody*> attached_bodies;
320  if (!ps)
321  {
322  ROS_ERROR("No planning scene");
323  return;
324  }
325  const moveit::core::JointModelGroup* jmg = ps->getCurrentState().getJointModelGroup(group_name);
326  if (jmg)
327  ps->getCurrentState().getAttachedBodies(attached_bodies, jmg);
328 
329  if (attached_bodies.empty())
330  {
331  ROS_ERROR("No bodies to place");
332  return;
333  }
334 
335  geometry_msgs::Quaternion upright_orientation;
336  upright_orientation.w = 1.0;
337 
338  // Else place the first one
339  place_poses_.clear();
340  place_poses_ = semantic_world_->generatePlacePoses(support_surface_name_, attached_bodies[0]->getShapes()[0],
341  upright_orientation, 0.1);
343  place_object_name_ = attached_bodies[0]->getName();
344  planning_display_->addBackgroundJob([this] { placeObject(); }, "place");
345 }
346 
348 {
349  std::string group_name = planning_display_->getCurrentPlanningGroup();
350  ui_->pick_button->setEnabled(false);
351  if (pick_object_name_.find(group_name) == pick_object_name_.end())
352  {
353  ROS_ERROR("No pick object set for this group");
354  return;
355  }
356  if (!support_surface_name_.empty())
357  {
358  move_group_->setSupportSurfaceName(support_surface_name_);
359  }
360  if (move_group_->pick(pick_object_name_[group_name]))
361  {
362  ui_->place_button->setEnabled(true);
363  }
364 }
365 
367 {
369  return;
370 }
371 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::MotionPlanningFrame::pick_object_name_
std::map< std::string, std::string > pick_object_name_
Definition: motion_planning_frame.h:284
moveit_rviz_plugin::MotionPlanningFrame::selectedDetectedObjectChanged
void selectedDetectedObjectChanged()
Definition: motion_planning_frame_manipulation.cpp:140
moveit_rviz_plugin::MotionPlanningFrame::pickObjectButtonClicked
void pickObjectButtonClicked()
Definition: motion_planning_frame_manipulation.cpp:296
motion_planning_display.h
moveit_rviz_plugin::MotionPlanningFrame::updateTables
void updateTables()
Definition: motion_planning_frame_manipulation.cpp:228
tf2_eigen.h
utils.h
moveit_rviz_plugin::MotionPlanningFrame::support_surface_name_
std::string support_surface_name_
Definition: motion_planning_frame.h:291
ROS_DEBUG
#define ROS_DEBUG(...)
moveit_rviz_plugin::MotionPlanningFrame::object_recognition_client_
std::unique_ptr< actionlib::SimpleActionClient< object_recognition_msgs::ObjectRecognitionAction > > object_recognition_client_
Definition: motion_planning_frame.h:297
moveit_rviz_plugin::PlanningSceneDisplay::addMainLoopJob
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
Definition: planning_scene_display.cpp:265
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
moveit_rviz_plugin::MotionPlanningFrame::pickObject
void pickObject()
Definition: motion_planning_frame_manipulation.cpp:379
moveit_rviz_plugin::MotionPlanningFrame::processDetectedObjects
void processDetectedObjects()
Definition: motion_planning_frame_manipulation.cpp:100
moveit_rviz_plugin::MotionPlanningFrame::publishTables
void publishTables()
Definition: motion_planning_frame_manipulation.cpp:234
planning_scene_monitor::LockedPlanningSceneRW
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
moveit_rviz_plugin::OBJECT_RECOGNITION_ACTION
const std::string OBJECT_RECOGNITION_ACTION
Definition: motion_planning_frame.h:86
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRO
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
Definition: planning_scene_display.cpp:328
moveit_rviz_plugin::MotionPlanningFrame::ui_
Ui::MotionPlanningUI * ui_
Definition: motion_planning_frame.h:127
moveit_rviz_plugin::MotionPlanningFrame::selected_object_name_
std::string selected_object_name_
Definition: motion_planning_frame.h:293
moveit_rviz_plugin::MotionPlanningFrame::place_poses_
std::vector< geometry_msgs::PoseStamped > place_poses_
Definition: motion_planning_frame.h:286
ROS_ERROR
#define ROS_ERROR(...)
moveit_rviz_plugin::MotionPlanningFrame::detectedObjectChanged
void detectedObjectChanged(QListWidgetItem *item)
Definition: motion_planning_frame_manipulation.cpp:164
moveit_rviz_plugin::MotionPlanningFrame::semantic_world_
moveit::semantic_world::SemanticWorldPtr semantic_world_
Definition: motion_planning_frame.h:131
actionlib::SimpleClientGoalState::SUCCEEDED
SUCCEEDED
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
moveit_rviz_plugin::PlanningSceneDisplay::addBackgroundJob
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
Definition: planning_scene_display.cpp:255
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::MotionPlanningFrame::updateSupportSurfacesList
void updateSupportSurfacesList()
Definition: motion_planning_frame_manipulation.cpp:264
moveit_rviz_plugin::MotionPlanningFrame::waitForAction
void waitForAction(const T &action, const ros::Duration &wait_for_server, const std::string &name)
Definition: motion_planning_frame.h:339
ros::Time
moveit_rviz_plugin::MotionPlanningFrame::listenDetectedObjects
void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr &msg)
Definition: motion_planning_frame_manipulation.cpp:198
moveit_rviz_plugin::MotionPlanningFrame::placeObjectButtonClicked
void placeObjectButtonClicked()
Definition: motion_planning_frame_manipulation.cpp:333
planning_scene_monitor::LockedPlanningSceneRO
moveit_rviz_plugin::MotionPlanningDisplay::visualizePlaceLocations
void visualizePlaceLocations(const std::vector< geometry_msgs::PoseStamped > &place_poses)
Definition: motion_planning_display.cpp:1480
moveit_rviz_plugin::MotionPlanningFrame::updateDetectedObjectsList
void updateDetectedObjectsList(const std::vector< std::string > &object_ids)
Definition: motion_planning_frame_manipulation.cpp:204
moveit::core::JointModelGroup
moveit_rviz_plugin::MotionPlanningFrame::triggerObjectDetection
void triggerObjectDetection()
Definition: motion_planning_frame_manipulation.cpp:168
moveit_rviz_plugin::MotionPlanningDisplay::getCurrentPlanningGroup
std::string getCurrentPlanningGroup() const
Definition: motion_planning_display.cpp:1083
tf2::toMsg
B toMsg(const A &a)
conversions.h
moveit_rviz_plugin::MotionPlanningFrame::planning_display_
MotionPlanningDisplay * planning_display_
Definition: motion_planning_frame.h:125
ros::Duration::sleep
bool sleep() const
moveit_rviz_plugin::MotionPlanningFrame::placeObject
void placeObject()
Definition: motion_planning_frame_manipulation.cpp:398
moveit_rviz_plugin::MotionPlanningFrame::selected_support_surface_name_
std::string selected_support_surface_name_
Definition: motion_planning_frame.h:294
ROS_INFO
#define ROS_INFO(...)
moveit_rviz_plugin::MotionPlanningFrame::selectedSupportSurfaceChanged
void selectedSupportSurfaceChanged()
Definition: motion_planning_frame_manipulation.cpp:240
motion_planning_frame.h
ros::Duration
moveit_rviz_plugin::MotionPlanningFrame::place_object_name_
std::string place_object_name_
Definition: motion_planning_frame.h:285
moveit_rviz_plugin::MotionPlanningFrame::move_group_
moveit::planning_interface::MoveGroupInterfacePtr move_group_
Definition: motion_planning_frame.h:130
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneRW
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
Definition: planning_scene_display.cpp:333
ros::Time::now
static Time now()
moveit_rviz_plugin::MotionPlanningFrame::detectObjectsButtonClicked
void detectObjectsButtonClicked()
Definition: motion_planning_frame_manipulation.cpp:83


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:25