42 #include <object_recognition_msgs/ObjectRecognitionGoal.h>
46 #include "ui_motion_planning_rviz_plugin_frame.h"
58 semantic_world_ = std::make_shared<moveit::semantic_world::SemanticWorld>(ps);
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;
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;
87 const collision_detection::WorldConstPtr& world = ps->getWorld();
89 for (
const auto&
object : *world)
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)
95 object_ids.push_back(
object.first);
98 if (!object_ids.empty())
104 ROS_DEBUG(
"Found %d objects", (
int)object_ids.size());
110 QList<QListWidgetItem*> sel =
ui_->detected_objects_list->selectedItems();
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;
141 std::make_unique<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction>>(
147 catch (std::exception& ex)
149 ROS_ERROR(
"Object recognition action: %s", ex.what());
153 object_recognition_msgs::ObjectRecognitionGoal goal;
174 ui_->detected_objects_list->setUpdatesEnabled(
false);
175 bool old_state =
ui_->detected_objects_list->blockSignals(
true);
176 ui_->detected_objects_list->clear();
178 for (std::size_t i = 0; i < object_ids.size(); ++i)
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);
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);
210 QList<QListWidgetItem*> sel =
ui_->support_surfaces_list->selectedItems();
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;
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;
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());
244 ui_->support_surfaces_list->setUpdatesEnabled(
false);
245 bool old_state =
ui_->support_surfaces_list->blockSignals(
true);
246 ui_->support_surfaces_list->clear();
248 for (std::size_t i = 0; i < support_ids.size(); ++i)
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);
259 ui_->support_surfaces_list->blockSignals(old_state);
260 ui_->support_surfaces_list->setUpdatesEnabled(
true);
266 QList<QListWidgetItem*> sel =
ui_->detected_objects_list->selectedItems();
267 QList<QListWidgetItem*> sel_table =
ui_->support_surfaces_list->selectedItems();
277 if (!sel_table.empty())
303 QList<QListWidgetItem*> sel_table =
ui_->support_surfaces_list->selectedItems();
306 if (!sel_table.empty())
311 ROS_ERROR(
"Need to specify table to place object on");
315 ui_->pick_button->setEnabled(
false);
316 ui_->place_button->setEnabled(
false);
318 std::vector<const moveit::core::AttachedBody*> attached_bodies;
327 ps->getCurrentState().getAttachedBodies(attached_bodies, jmg);
329 if (attached_bodies.empty())
335 geometry_msgs::Quaternion upright_orientation;
336 upright_orientation.w = 1.0;
341 upright_orientation, 0.1);
350 ui_->pick_button->setEnabled(
false);
353 ROS_ERROR(
"No pick object set for this group");
362 ui_->place_button->setEnabled(
true);